From 247d4d11ebd7b4102b311fa7f989eed6d872e64d Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 1 Apr 2024 13:13:28 -0400 Subject: [PATCH 001/126] initial add planning and execution frames for bases commit --- .../base/kinematicbase/differentialDrive.go | 4 + .../base/kinematicbase/fake_kinematics.go | 146 ++++++++++++------ components/base/kinematicbase/kinematics.go | 4 +- .../base/kinematicbase/ptgKinematics.go | 30 ++-- motionplan/motionPlanner.go | 27 ++-- motionplan/planManager.go | 14 +- referenceframe/model.go | 6 + services/motion/builtin/move_request.go | 25 ++- 8 files changed, 168 insertions(+), 88 deletions(-) diff --git a/components/base/kinematicbase/differentialDrive.go b/components/base/kinematicbase/differentialDrive.go index 3bc1ca05834..02344dd607e 100644 --- a/components/base/kinematicbase/differentialDrive.go +++ b/components/base/kinematicbase/differentialDrive.go @@ -111,6 +111,10 @@ func (ddk *differentialDriveKinematics) Kinematics() referenceframe.Frame { return ddk.planningFrame } +func (ddk *differentialDriveKinematics) ExecutionFrame() referenceframe.Frame { + return ddk.executionFrame +} + 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. diff --git a/components/base/kinematicbase/fake_kinematics.go b/components/base/kinematicbase/fake_kinematics.go index ef358858785..2cb2f446240 100644 --- a/components/base/kinematicbase/fake_kinematics.go +++ b/components/base/kinematicbase/fake_kinematics.go @@ -5,6 +5,7 @@ package kinematicbase import ( "context" "errors" + "fmt" "sync" "time" @@ -79,6 +80,10 @@ func (fk *fakeDiffDriveKinematics) Kinematics() referenceframe.Frame { return fk.planningFrame } +func (fk *fakeDiffDriveKinematics) ExecutionFrame() referenceframe.Frame { + return fk.executionFrame +} + func (fk *fakeDiffDriveKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { fk.lock.RLock() defer fk.lock.RUnlock() @@ -118,17 +123,17 @@ func (fk *fakeDiffDriveKinematics) CurrentPosition(ctx context.Context) (*refere type fakePTGKinematics struct { *fake.Base - localizer motion.Localizer - frame referenceframe.Frame - options Options - sensorNoise spatialmath.Pose - ptgs []tpspace.PTGSolver - currentInput []referenceframe.Input - origin *referenceframe.PoseInFrame - positionlock sync.RWMutex - inputLock sync.RWMutex - logger logging.Logger - sleepTime int + localizer motion.Localizer + planning, execution referenceframe.Frame + options Options + sensorNoise spatialmath.Pose + ptgs []tpspace.PTGSolver + currentInput []referenceframe.Input + origin *referenceframe.PoseInFrame + positionlock sync.RWMutex + inputLock sync.RWMutex + logger logging.Logger + sleepTime int } // WrapWithFakePTGKinematics creates a PTG KinematicBase from the fake Base so that it satisfies the ModelFramer and InputEnabled @@ -174,7 +179,8 @@ func WrapWithFakePTGKinematics( nonzeroBaseTurningRadiusMeters := (baseMillimetersPerSecond / rdkutils.DegToRad(angVelocityDegsPerSecond)) / 1000. - frame, err := tpspace.NewPTGFrameFromKinematicOptions( + // create planning frame + planningFrame, err := tpspace.NewPTGFrameFromKinematicOptions( b.Name().ShortName(), logger, nonzeroBaseTurningRadiusMeters, @@ -187,21 +193,29 @@ func WrapWithFakePTGKinematics( return nil, err } + // create execution frame + // should i change this so that it takes in a list of geometries? + executionFrame, err := referenceframe.New2DMobileModelFrame(b.Name().ShortName()+"ExecutionFrame", planningFrame.DoF(), geometries[0]) + if err != nil { + return nil, err + } + + // set other params if sensorNoise == nil { sensorNoise = spatialmath.NewZeroPose() } - ptgProv, ok := frame.(tpspace.PTGProvider) + ptgProv, ok := planningFrame.(tpspace.PTGProvider) if !ok { return nil, errors.New("unable to cast ptgk frame to a PTG Provider") } - ptgs := ptgProv.PTGSolvers() fk := &fakePTGKinematics{ Base: b, - frame: frame, + planning: planningFrame, + execution: executionFrame, origin: origin, - ptgs: ptgs, + ptgs: ptgProv.PTGSolvers(), currentInput: zeroInput, sensorNoise: sensorNoise, logger: logger, @@ -215,7 +229,11 @@ func WrapWithFakePTGKinematics( } func (fk *fakePTGKinematics) Kinematics() referenceframe.Frame { - return fk.frame + return fk.planning +} + +func (fk *fakePTGKinematics) ExecutionFrame() referenceframe.Frame { + return fk.execution } func (fk *fakePTGKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { @@ -232,47 +250,75 @@ func (fk *fakePTGKinematics) GoToInputs(ctx context.Context, inputSteps ...[]ref }() for _, inputs := range inputSteps { - fk.positionlock.RLock() - startingPose := fk.origin - fk.positionlock.RUnlock() + err := fk.goToInputs(ctx, inputs) + if err != nil { + return err + } + // we could do a setUpdateMe function to control when inputs are composed + // the frame's starting position + newPose, err := fk.Kinematics().Transform(inputs) + if err != nil { + return err + } + fmt.Println("newPose: ", spatialmath.PoseToProtobuf(newPose)) + // if tpFrame, ok := fk.Kinematics().(*tpspace.PtgGroupFrame); ok { + // tpFrame.SetStartPose(newPose) + // } else { + // return errors.New("something bad happened2") + // } + + } + return nil +} + +func (fk *fakePTGKinematics) goToInputs(ctx context.Context, inputs []referenceframe.Input) error { + defer func() { fk.inputLock.Lock() - fk.currentInput = []referenceframe.Input{inputs[0], inputs[1], {Value: 0}} + fk.currentInput = zeroInput fk.inputLock.Unlock() + }() + + fk.positionlock.RLock() + startingPose := fk.origin + fk.positionlock.RUnlock() + + fk.inputLock.Lock() + fk.currentInput = []referenceframe.Input{inputs[0], inputs[1], {Value: 0}} + fk.inputLock.Unlock() + + finalPose, err := fk.planning.Transform(inputs) + if err != nil { + return err + } - finalPose, err := fk.frame.Transform(inputs) + steps := motionplan.PathStepCount(spatialmath.NewZeroPose(), finalPose, 2) + startCfg := referenceframe.FloatsToInputs([]float64{inputs[0].Value, inputs[1].Value, 0}) + var interpolatedConfigurations [][]referenceframe.Input + for i := 0; i <= steps; i++ { + interp := float64(i) / float64(steps) + interpConfig := referenceframe.InterpolateInputs(startCfg, inputs, interp) + interpolatedConfigurations = append(interpolatedConfigurations, interpConfig) + } + for _, inter := range interpolatedConfigurations { + if ctx.Err() != nil { + return ctx.Err() + } + relativePose, err := fk.planning.Transform(inter) if err != nil { return err } + newPose := spatialmath.Compose(startingPose.Pose(), relativePose) - steps := motionplan.PathStepCount(spatialmath.NewZeroPose(), finalPose, 2) - startCfg := referenceframe.FloatsToInputs([]float64{inputs[0].Value, inputs[1].Value, 0}) - var interpolatedConfigurations [][]referenceframe.Input - for i := 0; i <= steps; i++ { - interp := float64(i) / float64(steps) - interpConfig := referenceframe.InterpolateInputs(startCfg, inputs, interp) - interpolatedConfigurations = append(interpolatedConfigurations, interpConfig) - } - for _, inter := range interpolatedConfigurations { - if ctx.Err() != nil { - return ctx.Err() - } - relativePose, err := fk.frame.Transform(inter) - if err != nil { - return err - } - newPose := spatialmath.Compose(startingPose.Pose(), relativePose) - - fk.positionlock.Lock() - fk.origin = referenceframe.NewPoseInFrame(fk.origin.Parent(), newPose) - fk.positionlock.Unlock() - - fk.inputLock.Lock() - fk.currentInput = []referenceframe.Input{inputs[0], inputs[1], inter[2]} - fk.inputLock.Unlock() - - time.Sleep(time.Duration(fk.sleepTime) * time.Microsecond * 10) - } + fk.positionlock.Lock() + fk.origin = referenceframe.NewPoseInFrame(fk.origin.Parent(), newPose) + fk.positionlock.Unlock() + + fk.inputLock.Lock() + fk.currentInput = []referenceframe.Input{inputs[0], inputs[1], inter[2]} + fk.inputLock.Unlock() + + time.Sleep(time.Duration(fk.sleepTime) * time.Microsecond * 10) } return nil } diff --git a/components/base/kinematicbase/kinematics.go b/components/base/kinematicbase/kinematics.go index 5242a37679e..d2089bc022f 100644 --- a/components/base/kinematicbase/kinematics.go +++ b/components/base/kinematicbase/kinematics.go @@ -23,7 +23,9 @@ type KinematicBase interface { motion.Localizer referenceframe.InputEnabled - Kinematics() referenceframe.Frame + Kinematics() referenceframe.Frame // this should be remaned to PlanningFrame()? + ExecutionFrame() referenceframe.Frame + // we should add ExecutionFrame() as a method on this interface // ErrorState takes a complete motionplan, as well as the index of the currently-executing set of inputs, and computes the pose // difference between where the robot in fact is, and where it ought to be. ErrorState(context.Context, motionplan.Plan, int) (spatialmath.Pose, error) diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index 1cd03a45e44..90d4792fbd4 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -42,9 +42,9 @@ const ( type ptgBaseKinematics struct { base.Base motion.Localizer - logger logging.Logger - frame referenceframe.Frame - ptgs []tpspace.PTGSolver + logger logging.Logger + planningFrame, executionFrame referenceframe.Frame + ptgs []tpspace.PTGSolver linVelocityMMPerSecond float64 angVelocityDegsPerSecond float64 @@ -112,7 +112,7 @@ func wrapWithPTGKinematics( } nonzeroBaseTurningRadiusMeters := (linVelocityMMPerSecond / rdkutils.DegToRad(angVelocityDegsPerSecond)) / 1000. - frame, err := tpspace.NewPTGFrameFromKinematicOptions( + planningFrame, err := tpspace.NewPTGFrameFromKinematicOptions( b.Name().ShortName(), logger, nonzeroBaseTurningRadiusMeters, @@ -125,7 +125,14 @@ func wrapWithPTGKinematics( return nil, err } - ptgProv, ok := frame.(tpspace.PTGProvider) + // create execution frame + // should i change this so that it takes in a list of geometries? + executionFrame, err := referenceframe.New2DMobileModelFrame(b.Name().ShortName(), planningFrame.DoF(), geometries[0]) + if err != nil { + return nil, err + } + + ptgProv, ok := planningFrame.(tpspace.PTGProvider) if !ok { return nil, errors.New("unable to cast ptgk frame to a PTG Provider") } @@ -143,7 +150,8 @@ func wrapWithPTGKinematics( Base: b, Localizer: localizer, logger: logger, - frame: frame, + planningFrame: planningFrame, + executionFrame: executionFrame, ptgs: ptgs, linVelocityMMPerSecond: linVelocityMMPerSecond, angVelocityDegsPerSecond: angVelocityDegsPerSecond, @@ -154,7 +162,11 @@ func wrapWithPTGKinematics( } func (ptgk *ptgBaseKinematics) Kinematics() referenceframe.Frame { - return ptgk.frame + return ptgk.planningFrame +} + +func (ptgk *ptgBaseKinematics) ExecutionFrame() referenceframe.Frame { + return ptgk.executionFrame } func (ptgk *ptgBaseKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { @@ -275,7 +287,7 @@ func (ptgk *ptgBaseKinematics) ErrorState(ctx context.Context, plan motionplan.P // TODO: We should be able to use the Path that exists in the plan rather than doing this duplicate work here runningPose := spatialmath.NewZeroPose() for i := 0; i < currentNode; i++ { - wpPose, err := ptgk.frame.Transform(waypoints[i]) + wpPose, err := ptgk.planningFrame.Transform(waypoints[i]) if err != nil { return nil, err } @@ -287,7 +299,7 @@ func (ptgk *ptgBaseKinematics) ErrorState(ctx context.Context, plan motionplan.P if err != nil { return nil, err } - currPose, err := ptgk.frame.Transform(currentInputs) + currPose, err := ptgk.planningFrame.Transform(currentInputs) if err != nil { return nil, err } diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 08501366adc..138b0bd6378 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -471,19 +471,9 @@ func CheckPlan( return err } - var startPose spatialmath.Pose - if relative { - // A frame's transformation based on a relative input will position it relative to the - // frame's origin, giving us a relative pose. To put it with respect to the world - // we compose the relative pose with the most recent former pose we have already reached. - startPose = poses[wayPointIdx-1] - } else { - startPose = currentPose - } - // setup the planOpts if sfPlanner.planOpts, err = sfPlanner.plannerSetupFromMoveRequest( - startPose, + currentPose, poses[len(poses)-1], currentInputs, worldState, @@ -494,6 +484,7 @@ func CheckPlan( } // create a list of segments to iterate through + // doesn't this need to also happen for absolute plans?? segments := make([]*ik.Segment, 0, len(poses)-wayPointIdx) if relative { // get the inputs we were partway through executing @@ -571,6 +562,7 @@ func CheckPlan( if err != nil { return err } + fmt.Println("poseInPath: ", spatialmath.PoseToProtobuf(poseInPath)) // Check if look ahead distance has been reached currentTravelDistanceMM := totalTravelDistanceMM + poseInPath.Point().Distance(segment.StartPosition.Point()) @@ -578,13 +570,16 @@ func CheckPlan( 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. + // Add new comment here interpolatedState := &ik.State{Frame: sf} if relative { - interpolatedState.Position = spatialmath.Compose(segment.StartPosition, poseInPath) + // I AM PRETTY SURE THAT ORDER MATTERS HERE IN TERMS OF STUFF? + interpolatedState.Configuration = []frame.Input{ + {Value: segment.StartPosition.Point().X}, + {Value: segment.StartPosition.Point().Y}, + {Value: segment.StartPosition.Orientation().OrientationVectorDegrees().Theta}, + interpConfig[0], interpConfig[1], interpConfig[2], + } } else { interpolatedState.Configuration = interpConfig } diff --git a/motionplan/planManager.go b/motionplan/planManager.go index e1f481d9de1..e049ea14bc4 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -506,17 +506,9 @@ 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 + fmt.Println("PRINTING movingRobotGeometries") + for _, g := range movingRobotGeometries { + fmt.Println("g.Pose(): ", spatialmath.PoseToProtobuf(g.Pose())) } // find all geometries that are not moving but are in the frame system diff --git a/referenceframe/model.go b/referenceframe/model.go index eeec720bcf5..1c2c3a731f3 100644 --- a/referenceframe/model.go +++ b/referenceframe/model.go @@ -71,6 +71,12 @@ func (m *SimpleModel) Transform(inputs []Input) (spatialmath.Pose, error) { if err != nil && frames == nil { return nil, err } + // this confuses me a lot + // why not just + // return spatialmath.NewPose( + // r3.Vector{X: inputs[0].Value, Y: inputs[1].Value, Z: 0}, + // &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: inputs[2].Value}, + // ), nil return frames[0].transform, err } diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index bf8d20d80e7..8b69c8c717b 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -69,6 +69,7 @@ type moveRequest struct { obstacleDetectors map[vision.Service][]resource.Name replanCostFactor float64 fsService framesystem.Service + collisionFS referenceframe.FrameSystem executeBackgroundWorkers *sync.WaitGroup responseChan chan moveResponse @@ -311,6 +312,15 @@ func (mr *moveRequest) obstaclesIntersectPlan( waypointIndex := mr.waypointIndex mr.waypointIndexMutex.Unlock() + for _, pif := range plan.Path()[waypointIndex-1] { + inputMap[mr.kinematicBase.Name().ShortName()+"ExecutionFrame"] = + referenceframe.FloatsToInputs([]float64{ + pif.Pose().Point().X, + pif.Pose().Point().Y, + pif.Pose().Orientation().OrientationVectorDegrees().Theta, + }) + } + // get the pose difference between where the robot is versus where it ought to be. errorState, err := mr.kinematicBase.ErrorState(ctx, plan, waypointIndex) if err != nil { @@ -329,7 +339,7 @@ func (mr *moveRequest) obstaclesIntersectPlan( plan, waypointIndex, worldState, // detected obstacles by this instance of camera + service - mr.planRequest.FrameSystem, + mr.collisionFS, currentPosition.Pose(), // currentPosition of robot accounts for errorState inputMap, errorState, // deviation of robot from plan @@ -698,12 +708,24 @@ func (ms *builtIn) createBaseMoveRequest( return nil, err } } + // Construct framesystem used for planning // We want to disregard anything in the FS whose eventual parent is not the base, because we don't know where it is. baseOnlyFS, err := fs.FrameSystemSubset(kinematicFrame) if err != nil { return nil, err } + // Construct framesystem used for CheckPlan + collisionFS := referenceframe.NewEmptyFrameSystem("collisionFS") + err = collisionFS.AddFrame(kb.ExecutionFrame(), collisionFS.World()) + if err != nil { + return nil, err + } + err = collisionFS.MergeFrameSystem(baseOnlyFS, kb.ExecutionFrame()) + if err != nil { + return nil, err + } + startPoseIF, err := kb.CurrentPosition(ctx) if err != nil { return nil, err @@ -791,6 +813,7 @@ func (ms *builtIn) createBaseMoveRequest( replanCostFactor: valExtra.replanCostFactor, obstacleDetectors: obstacleDetectors, fsService: ms.fsService, + collisionFS: collisionFS, executeBackgroundWorkers: &backgroundWorkers, From 652ddcc27a421748d16c61f09c15aca179f78be4 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Tue, 2 Apr 2024 11:12:56 -0400 Subject: [PATCH 002/126] continue with solverFrame - not the best idea --- components/base/fake/fake.go | 2 + .../base/kinematicbase/fake_kinematics.go | 5 +- motionplan/constraint.go | 4 + motionplan/motionPlanner.go | 5 + motionplan/planManager.go | 16 +++ motionplan/solverFrame.go | 107 ++++++++++++++++-- motionplan/tpspace/ptgGroupFrame.go | 1 + referenceframe/model.go | 2 + services/motion/builtin/builtin_utils_test.go | 2 +- 9 files changed, 134 insertions(+), 10 deletions(-) diff --git a/components/base/fake/fake.go b/components/base/fake/fake.go index 076f6f7f59c..69aa03142a4 100644 --- a/components/base/fake/fake.go +++ b/components/base/fake/fake.go @@ -3,6 +3,7 @@ package fake import ( "context" + "fmt" "github.com/golang/geo/r3" @@ -50,6 +51,7 @@ func NewBase(_ context.Context, _ resource.Dependencies, conf resource.Config, l if err != nil { return nil, err } + fmt.Println("geometry.Label(): ", geometry.Label()) b.Geometry = []spatialmath.Geometry{geometry} } b.WidthMeters = defaultWidthMm * 0.001 diff --git a/components/base/kinematicbase/fake_kinematics.go b/components/base/kinematicbase/fake_kinematics.go index 2cb2f446240..4f7afe35ab4 100644 --- a/components/base/kinematicbase/fake_kinematics.go +++ b/components/base/kinematicbase/fake_kinematics.go @@ -180,6 +180,7 @@ func WrapWithFakePTGKinematics( nonzeroBaseTurningRadiusMeters := (baseMillimetersPerSecond / rdkutils.DegToRad(angVelocityDegsPerSecond)) / 1000. // create planning frame + fmt.Println("b.Name().ShortName(): ", b.Name().ShortName()) planningFrame, err := tpspace.NewPTGFrameFromKinematicOptions( b.Name().ShortName(), logger, @@ -195,7 +196,9 @@ func WrapWithFakePTGKinematics( // create execution frame // should i change this so that it takes in a list of geometries? - executionFrame, err := referenceframe.New2DMobileModelFrame(b.Name().ShortName()+"ExecutionFrame", planningFrame.DoF(), geometries[0]) + executionFrame, err := referenceframe.New2DMobileModelFrame( + b.Name().ShortName()+"ExecutionFrame", + planningFrame.DoF(), geometries[0]) if err != nil { return nil, err } diff --git a/motionplan/constraint.go b/motionplan/constraint.go index ab2e7dbb3f7..471c7198aa3 100644 --- a/motionplan/constraint.go +++ b/motionplan/constraint.go @@ -332,6 +332,10 @@ func NewCollisionConstraint( return false } internalGeoms = internal.Geometries() + // for _, g := range internalGeoms { + // fmt.Println("g.String(): ", g.String()) + // fmt.Println("g.Pose(): ", spatial.PoseToProtobuf(g.Pose())) + // } case state.Position != nil: // If we didn't pass a Configuration, but we do have a Position, then get the geometries at the zero state and // transform them to the Position diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 138b0bd6378..88f247d3723 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -443,6 +443,10 @@ func CheckPlan( return errors.New("plan must have at least one element") } + fmt.Println("have the following FrameSystem") + fmt.Println("fs.FrameNames(): ", fs.FrameNames()) + fmt.Println("currentInputs: ", currentInputs) + // construct solverFrame // Note that this requires all frames which move as part of the plan, to have an // entry in the very first plan waypoint @@ -583,6 +587,7 @@ func CheckPlan( } else { interpolatedState.Configuration = interpConfig } + fmt.Println("interpolatedState: ", interpolatedState) // 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 { diff --git a/motionplan/planManager.go b/motionplan/planManager.go index e049ea14bc4..355588331ef 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -38,6 +38,7 @@ type planManager struct { activeBackgroundWorkers sync.WaitGroup useTPspace bool + // ugh, we might need to add another bool here??? } func newPlanManager( @@ -506,9 +507,24 @@ 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 + } fmt.Println("PRINTING movingRobotGeometries") for _, g := range movingRobotGeometries { + fmt.Println("g.String(): ", g.String()) + fmt.Println("g.Label(): ", g.Label()) fmt.Println("g.Pose(): ", spatialmath.PoseToProtobuf(g.Pose())) + fmt.Println("-----------------------") } // find all geometries that are not moving but are in the frame system diff --git a/motionplan/solverFrame.go b/motionplan/solverFrame.go index 32f390cdcd3..9e61a00abac 100644 --- a/motionplan/solverFrame.go +++ b/motionplan/solverFrame.go @@ -2,6 +2,7 @@ package motionplan import ( "errors" + "fmt" "go.uber.org/multierr" pb "go.viam.com/api/component/arm/v1" @@ -42,6 +43,8 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, return nil, err } + // is the solution that frames which are above the solveFrameName should be not worried about? + // get solve frame solveFrame := fs.Frame(solveFrameName) if solveFrame == nil { @@ -51,6 +54,10 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, if err != nil { return nil, err } + fmt.Println("printing solveFrameList", solveFrameList) + for _, f := range solveFrameList { + fmt.Println("f.Name(): ", f.Name()) + } if len(solveFrameList) == 0 { return nil, errors.New("solveFrameList was empty") } @@ -78,6 +85,7 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, if err != nil { return nil, err } + fmt.Println("pivotFrame: ", pivotFrame.Name()) if pivotFrame.Name() == frame.World { frames = uniqInPlaceSlice(append(solveFrameList, goalFrameList...)) moving, err = movingFS(solveFrameList) @@ -148,8 +156,9 @@ 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 + // TODO(nf): uncomment + // anyNonzero := false // Whether non-PTG frames for _, movingFrame := range frames { if ptgFrame, isPTGframe := movingFrame.(tpspace.PTGProvider); isPTGframe { if anyPTG { @@ -157,12 +166,12 @@ 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") - } + } // 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") + // } } return &solverFrame{ @@ -235,6 +244,14 @@ func (sf *solverFrame) Geometries(inputs []frame.Input) (*frame.GeometriesInFram var errAll error inputMap := sf.sliceToMap(inputs) sfGeometries := []spatial.Geometry{} + + // i think this function will need to be changed + // if we are relative we know so + // we then assume that sf.name is the name of the kinematic base and anything above it should be + // treated as a static transform frame + + // all other frame, we do not need to worry about and they should be treated as normal + for _, fName := range sf.movingFS.FrameNames() { f := sf.fss.Frame(fName) if f == nil { @@ -245,6 +262,80 @@ func (sf *solverFrame) Geometries(inputs []frame.Input) (*frame.GeometriesInFram return nil, err } gf, err := f.Geometries(inputs) + fmt.Println("about to print f.Geometries(inputs)") + for _, g := range gf.Geometries() { + fmt.Println("g.Label(): ", g.Label()) + } + if gf == nil { + // only propagate errors that result in nil geometry + multierr.AppendInto(&errAll, err) + continue + } + var tf frame.Transformable + tf, err = sf.fss.Transform(inputMap, gf, frame.World) + if err != nil { + return nil, err + } + sfGeometries = append(sfGeometries, tf.(*frame.GeometriesInFrame).Geometries()...) + } + return frame.NewGeometriesInFrame(frame.World, sfGeometries), errAll +} + +func (sf *solverFrame) MYGeometries(inputs []frame.Input) (*frame.GeometriesInFrame, error) { + if len(inputs) != len(sf.DoF()) { + return nil, frame.NewIncorrectInputLengthError(len(inputs), len(sf.DoF())) + } + var errAll error + inputMap := sf.sliceToMap(inputs) + sfGeometries := []spatial.Geometry{} + + // first we check if this is a solverFrame which with we need to concern ourselves with for Ptgs + isRelative := len(sf.ptgs) > 0 + if isRelative { + solveFrameList, err := sf.fss.TracebackFrame(sf.solveFrame) + if err != nil { + return nil, err + } + // we can probably just explicitly check the length of the list here + // it should only be wrld -- executionFrame -- planningFrame + // further, I believe that the indicies at which these frames are found shall remain constant + // hence, we can simplifify our code here + // todo: need to figure out the inidicied which are proper + for _, f := range solveFrameList { + inputs, err := frame.GetFrameInputs(f, inputMap) + if err != nil { + return nil, err + } + gf, err := f.Geometries(inputs) + if gf == nil { + // only propagate errors that result in nil geometry + multierr.AppendInto(&errAll, err) + continue + } + } + } + + // i think this function will need to be changed + // if we are relative we know so + // we then assume that sf.name is the name of the kinematic base and anything above it should be + // treated as a static transform frame + + // all other frame, we do not need to worry about and they should be treated as normal + + for _, fName := range sf.movingFS.FrameNames() { + f := sf.fss.Frame(fName) + if f == nil { + return nil, frame.NewFrameMissingError(fName) + } + inputs, err := frame.GetFrameInputs(f, inputMap) + if err != nil { + return nil, err + } + gf, err := f.Geometries(inputs) + fmt.Println("about to print f.Geometries(inputs)") + for _, g := range gf.Geometries() { + fmt.Println("g.Label(): ", g.Label()) + } if gf == nil { // only propagate errors that result in nil geometry multierr.AppendInto(&errAll, err) diff --git a/motionplan/tpspace/ptgGroupFrame.go b/motionplan/tpspace/ptgGroupFrame.go index 344856b4659..9f073dbb34e 100644 --- a/motionplan/tpspace/ptgGroupFrame.go +++ b/motionplan/tpspace/ptgGroupFrame.go @@ -184,6 +184,7 @@ func (pf *ptgGroupFrame) Geometries(inputs []referenceframe.Input) (*referencefr for _, geom := range pf.geometries { geoms = append(geoms, geom.Transform(transformedPose)) } + fmt.Println("geoms[0].Label(): ", geoms[0].Label()) return referenceframe.NewGeometriesInFrame(pf.name, geoms), nil } diff --git a/referenceframe/model.go b/referenceframe/model.go index 1c2c3a731f3..dcc92498e1b 100644 --- a/referenceframe/model.go +++ b/referenceframe/model.go @@ -3,6 +3,7 @@ package referenceframe import ( "encoding/binary" "encoding/json" + "fmt" "math" "math/rand" "strings" @@ -128,6 +129,7 @@ func (m *SimpleModel) Geometries(inputs []Input) (*GeometriesInFrame, error) { geometries = append(geometries, placedGeom) } } + fmt.Println("geometries[0].Label(): ", geometries[0].Label()) return NewGeometriesInFrame(m.name, geometries), errAll } diff --git a/services/motion/builtin/builtin_utils_test.go b/services/motion/builtin/builtin_utils_test.go index 3f1cea4b439..c23f32eb74b 100644 --- a/services/motion/builtin/builtin_utils_test.go +++ b/services/motion/builtin/builtin_utils_test.go @@ -237,7 +237,7 @@ func createMoveOnMapEnvironment( cfg := resource.Config{ Name: "test-base", API: base.API, - Frame: &referenceframe.LinkConfig{Geometry: &spatialmath.GeometryConfig{R: geomSize}}, + Frame: &referenceframe.LinkConfig{Geometry: &spatialmath.GeometryConfig{R: geomSize, Label: "kinematicBaseGeometry"}}, } logger := logging.NewTestLogger(t) fakeBase, err := baseFake.NewBase(ctx, nil, cfg, logger) From de3189d9011de81fbb11fe81b77fde70e513ae0a Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 5 Apr 2024 10:53:19 -0400 Subject: [PATCH 003/126] merge with main: --- .../base/kinematicbase/fake_kinematics.go | 88 ++++++++----------- components/base/kinematicbase/kinematics.go | 2 +- services/motion/builtin/move_request.go | 23 ----- 3 files changed, 37 insertions(+), 76 deletions(-) diff --git a/components/base/kinematicbase/fake_kinematics.go b/components/base/kinematicbase/fake_kinematics.go index af77f41ff5c..6f3b8840941 100644 --- a/components/base/kinematicbase/fake_kinematics.go +++ b/components/base/kinematicbase/fake_kinematics.go @@ -5,7 +5,6 @@ package kinematicbase import ( "context" "errors" - "fmt" "sync" "time" @@ -80,10 +79,6 @@ func (fk *fakeDiffDriveKinematics) Kinematics() referenceframe.Frame { return fk.planningFrame } -func (fk *fakeDiffDriveKinematics) ExecutionFrame() referenceframe.Frame { - return fk.executionFrame -} - func (fk *fakeDiffDriveKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { fk.lock.RLock() defer fk.lock.RUnlock() @@ -123,17 +118,17 @@ func (fk *fakeDiffDriveKinematics) CurrentPosition(ctx context.Context) (*refere type fakePTGKinematics struct { *fake.Base - localizer motion.Localizer - planning, execution referenceframe.Frame - options Options - sensorNoise spatialmath.Pose - ptgs []tpspace.PTGSolver - currentInput []referenceframe.Input - origin *referenceframe.PoseInFrame - positionlock sync.RWMutex - inputLock sync.RWMutex - logger logging.Logger - sleepTime int + localizer motion.Localizer + frame referenceframe.Frame + options Options + sensorNoise spatialmath.Pose + ptgs []tpspace.PTGSolver + currentInput []referenceframe.Input + origin *referenceframe.PoseInFrame + positionlock sync.RWMutex + inputLock sync.RWMutex + logger logging.Logger + sleepTime int } // WrapWithFakePTGKinematics creates a PTG KinematicBase from the fake Base so that it satisfies the ModelFramer and InputEnabled @@ -179,9 +174,7 @@ func WrapWithFakePTGKinematics( nonzeroBaseTurningRadiusMeters := (baseMillimetersPerSecond / rdkutils.DegToRad(angVelocityDegsPerSecond)) / 1000. - // create planning frame - fmt.Println("b.Name().ShortName(): ", b.Name().ShortName()) - planningFrame, err := tpspace.NewPTGFrameFromKinematicOptions( + frame, err := tpspace.NewPTGFrameFromKinematicOptions( b.Name().ShortName(), logger, nonzeroBaseTurningRadiusMeters, @@ -194,31 +187,21 @@ func WrapWithFakePTGKinematics( return nil, err } - // create execution frame - // should i change this so that it takes in a list of geometries? - executionFrame, err := referenceframe.New2DMobileModelFrame( - b.Name().ShortName()+"ExecutionFrame", - planningFrame.DoF(), geometries[0]) - if err != nil { - return nil, err - } - - // set other params if sensorNoise == nil { sensorNoise = spatialmath.NewZeroPose() } - ptgProv, ok := planningFrame.(tpspace.PTGProvider) + ptgProv, ok := frame.(tpspace.PTGProvider) if !ok { return nil, errors.New("unable to cast ptgk frame to a PTG Provider") } + ptgs := ptgProv.PTGSolvers() fk := &fakePTGKinematics{ Base: b, - planning: planningFrame, - execution: executionFrame, + frame: frame, origin: origin, - ptgs: ptgProv.PTGSolvers(), + ptgs: ptgs, currentInput: zeroInput, sensorNoise: sensorNoise, logger: logger, @@ -232,11 +215,7 @@ func WrapWithFakePTGKinematics( } func (fk *fakePTGKinematics) Kinematics() referenceframe.Frame { - return fk.planning -} - -func (fk *fakePTGKinematics) ExecutionFrame() referenceframe.Frame { - return fk.execution + return fk.frame } func (fk *fakePTGKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { @@ -253,14 +232,18 @@ func (fk *fakePTGKinematics) GoToInputs(ctx context.Context, inputSteps ...[]ref }() for _, inputs := range inputSteps { - fk.inputLock.Lock() - fk.currentInput = []referenceframe.Input{inputs[0], inputs[1], {Value: 0}} - fk.inputLock.Unlock() + fk.positionlock.RLock() + startingPose := fk.origin + fk.positionlock.RUnlock() - finalPose, err := fk.planning.Transform(inputs) - if err != nil { - return err - } + fk.inputLock.Lock() + fk.currentInput = []referenceframe.Input{inputs[0], inputs[1], {Value: 0}} + fk.inputLock.Unlock() + + finalPose, err := fk.frame.Transform(inputs) + if err != nil { + return err + } steps := motionplan.PathStepCount(spatialmath.NewZeroPose(), finalPose, 2) startCfg := referenceframe.FloatsToInputs([]float64{inputs[0].Value, inputs[1].Value, 0}) @@ -283,15 +266,16 @@ func (fk *fakePTGKinematics) GoToInputs(ctx context.Context, inputSteps ...[]ref } newPose := spatialmath.Compose(startingPose.Pose(), relativePose) - fk.positionlock.Lock() - fk.origin = referenceframe.NewPoseInFrame(fk.origin.Parent(), newPose) - fk.positionlock.Unlock() + fk.positionlock.Lock() + fk.origin = referenceframe.NewPoseInFrame(fk.origin.Parent(), newPose) + fk.positionlock.Unlock() - fk.inputLock.Lock() - fk.currentInput = []referenceframe.Input{inputs[0], inputs[1], inter[2]} - fk.inputLock.Unlock() + fk.inputLock.Lock() + fk.currentInput = []referenceframe.Input{inputs[0], inputs[1], inter[2]} + fk.inputLock.Unlock() - time.Sleep(time.Duration(fk.sleepTime) * time.Microsecond * 10) + time.Sleep(time.Duration(fk.sleepTime) * time.Microsecond * 10) + } } return nil } diff --git a/components/base/kinematicbase/kinematics.go b/components/base/kinematicbase/kinematics.go index a8f9c9c8ec6..b240b4bde81 100644 --- a/components/base/kinematicbase/kinematics.go +++ b/components/base/kinematicbase/kinematics.go @@ -23,7 +23,7 @@ type KinematicBase interface { referenceframe.InputEnabled Kinematics() referenceframe.Frame // this should be remaned to PlanningFrame()? - ExecutionFrame() referenceframe.Frame + // ExecutionFrame() referenceframe.Frame // we should add ExecutionFrame() as a method on this interface // ErrorState takes a complete motionplan, as well as the index of the currently-executing set of inputs, and computes the pose // difference between where the robot in fact is, and where it ought to be. diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 9cbf9fea5a0..6a5518529b8 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -67,7 +67,6 @@ type moveRequest struct { obstacleDetectors map[vision.Service][]resource.Name replanCostFactor float64 fsService framesystem.Service - collisionFS referenceframe.FrameSystem executeBackgroundWorkers *sync.WaitGroup responseChan chan moveResponse @@ -281,15 +280,6 @@ func (mr *moveRequest) obstaclesIntersectPlan( // versus when CheckPlan is actually called. // We load the wayPointIndex value to ensure that all information is up to date. - for _, pif := range plan.Path()[waypointIndex-1] { - inputMap[mr.kinematicBase.Name().ShortName()+"ExecutionFrame"] = - referenceframe.FloatsToInputs([]float64{ - pif.Pose().Point().X, - pif.Pose().Point().Y, - pif.Pose().Orientation().OrientationVectorDegrees().Theta, - }) - } - // get the pose difference between where the robot is versus where it ought to be. errorState, err := mr.kinematicBase.ErrorState(ctx) if err != nil { @@ -679,24 +669,12 @@ func (ms *builtIn) createBaseMoveRequest( return nil, err } } - // Construct framesystem used for planning // We want to disregard anything in the FS whose eventual parent is not the base, because we don't know where it is. baseOnlyFS, err := fs.FrameSystemSubset(kinematicFrame) if err != nil { return nil, err } - // Construct framesystem used for CheckPlan - collisionFS := referenceframe.NewEmptyFrameSystem("collisionFS") - err = collisionFS.AddFrame(kb.ExecutionFrame(), collisionFS.World()) - if err != nil { - return nil, err - } - err = collisionFS.MergeFrameSystem(baseOnlyFS, kb.ExecutionFrame()) - if err != nil { - return nil, err - } - startPoseIF, err := kb.CurrentPosition(ctx) if err != nil { return nil, err @@ -780,7 +758,6 @@ func (ms *builtIn) createBaseMoveRequest( replanCostFactor: valExtra.replanCostFactor, obstacleDetectors: obstacleDetectors, fsService: ms.fsService, - collisionFS: collisionFS, executeBackgroundWorkers: &backgroundWorkers, From 2aab60014b8d999221c58ced57592b6e3f5afa25 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Tue, 9 Apr 2024 14:15:56 -0400 Subject: [PATCH 004/126] initial commit - rework move_req to rely on fs for placing obstacles --- motionplan/planManager.go | 1 - services/motion/builtin/builtin_test.go | 108 +++++++++++++----------- services/motion/builtin/move_request.go | 46 +++++----- 3 files changed, 80 insertions(+), 75 deletions(-) diff --git a/motionplan/planManager.go b/motionplan/planManager.go index 355588331ef..bc6f592d9fb 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -521,7 +521,6 @@ func (pm *planManager) plannerSetupFromMoveRequest( } fmt.Println("PRINTING movingRobotGeometries") for _, g := range movingRobotGeometries { - fmt.Println("g.String(): ", g.String()) fmt.Println("g.Label(): ", g.Label()) fmt.Println("g.Pose(): ", spatialmath.PoseToProtobuf(g.Pose())) fmt.Println("-----------------------") diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 016032b2622..e0eba93d41a 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -512,8 +512,8 @@ func TestObstacleReplanningGlobe(t *testing.T) { } func TestObstacleReplanningSlam(t *testing.T) { - cameraToBase := spatialmath.NewPose(r3.Vector{0, 0, 0}, &spatialmath.OrientationVectorDegrees{OY: 1, Theta: -90}) - cameraToBaseInv := spatialmath.PoseInverse(cameraToBase) + // cameraToBase := spatialmath.NewPose(r3.Vector{0, 0, 0}, &spatialmath.OrientationVectorDegrees{OY: 1, Theta: -90}) + // cameraToBaseInv := spatialmath.PoseInverse(cameraToBase) ctx := context.Background() origin := spatialmath.NewPose( @@ -521,62 +521,68 @@ func TestObstacleReplanningSlam(t *testing.T) { &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: -90}, ) - boxWrld, err := spatialmath.NewBox( - spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 0, Z: 0}), - r3.Vector{X: 50, Y: 50, Z: 50}, "box-obstacle", - ) - test.That(t, err, test.ShouldBeNil) + // boxWrld, err := spatialmath.NewBox( + // spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 0, Z: 0}), + // r3.Vector{X: 50, Y: 50, Z: 50}, "box-obstacle", + // ) + // test.That(t, err, test.ShouldBeNil) - kb, ms := createMoveOnMapEnvironment( + _, ms := createMoveOnMapEnvironment( ctx, t, "pointcloud/cardboardOcto.pcd", 50, origin, ) defer ms.Close(ctx) - visSrvc, ok := ms.(*builtIn).visionServices[vision.Named("test-vision")].(*inject.VisionService) - test.That(t, ok, test.ShouldBeTrue) - i := 0 - visSrvc.GetObjectPointCloudsFunc = func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { - if i == 0 { - i++ - return []*viz.Object{}, nil - } - currentPif, err := kb.CurrentPosition(ctx) - test.That(t, err, test.ShouldBeNil) - - relativeBox := boxWrld.Transform(spatialmath.PoseInverse(currentPif.Pose())).Transform(cameraToBaseInv) - detection, err := viz.NewObjectWithLabel(pointcloud.New(), "test-case-1-detection", relativeBox.ToProtobuf()) - test.That(t, err, test.ShouldBeNil) - - return []*viz.Object{detection}, nil - } - - obstacleDetectorSlice := []motion.ObstacleDetectorName{ - {VisionServiceName: vision.Named("test-vision"), CameraName: camera.Named("test-camera")}, - } - req := motion.MoveOnMapReq{ - ComponentName: base.Named("test-base"), - Destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 800, Y: 0, Z: 0}), - SlamName: slam.Named("test_slam"), - MotionCfg: &motion.MotionConfiguration{ - PositionPollingFreqHz: 1, ObstaclePollingFreqHz: 100, PlanDeviationMM: 1, ObstacleDetectors: obstacleDetectorSlice, - }, - // TODO: add back "max_replans": 1 to extra - Extra: map[string]interface{}{"smooth_iter": 0}, - } - - executionID, err := ms.MoveOnMap(ctx, req) - test.That(t, err, test.ShouldBeNil) - - timeoutCtx, timeoutFn := context.WithTimeout(ctx, time.Second*15) - defer timeoutFn() - err = motion.PollHistoryUntilSuccessOrError(timeoutCtx, ms, time.Millisecond, motion.PlanHistoryReq{ - ComponentName: req.ComponentName, - ExecutionID: executionID, - LastPlanOnly: true, - }) + cI, _, err := ms.(*builtIn).fsService.CurrentInputs(ctx) test.That(t, err, test.ShouldBeNil) + fmt.Println("cI: ", cI) + + // visSrvc, ok := ms.(*builtIn).visionServices[vision.Named("test-vision")].(*inject.VisionService) + // test.That(t, ok, test.ShouldBeTrue) + // i := 0 + // visSrvc.GetObjectPointCloudsFunc = func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { + // if i == 0 { + // i++ + // return []*viz.Object{}, nil + // } + // currentPif, err := kb.CurrentPosition(ctx) + // test.That(t, err, test.ShouldBeNil) + + // relativeBox := boxWrld.Transform(spatialmath.PoseInverse(currentPif.Pose())).Transform(cameraToBaseInv) + // detection, err := viz.NewObjectWithLabel(pointcloud.New(), "test-case-1-detection", relativeBox.ToProtobuf()) + // test.That(t, err, test.ShouldBeNil) + + // return []*viz.Object{detection}, nil + // } + + // obstacleDetectorSlice := []motion.ObstacleDetectorName{ + // {VisionServiceName: vision.Named("test-vision"), CameraName: camera.Named("test-camera")}, + // } + // req := motion.MoveOnMapReq{ + // ComponentName: base.Named("test-base"), + // Destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 800, Y: 0, Z: 0}), + // SlamName: slam.Named("test_slam"), + // MotionCfg: &motion.MotionConfiguration{ + // PositionPollingFreqHz: 1, ObstaclePollingFreqHz: 100, PlanDeviationMM: 1, ObstacleDetectors: obstacleDetectorSlice, + // }, + // // TODO: add back "max_replans": 1 to extra + // Extra: map[string]interface{}{"smooth_iter": 0}, + // } + + // executionID, err := ms.MoveOnMap(ctx, req) + // test.That(t, err, test.ShouldBeNil) + + // timeoutCtx, timeoutFn := context.WithTimeout(ctx, time.Second*15) + // defer timeoutFn() + // + // err = motion.PollHistoryUntilSuccessOrError(timeoutCtx, ms, time.Millisecond, motion.PlanHistoryReq{ + // ComponentName: req.ComponentName, + // ExecutionID: executionID, + // LastPlanOnly: true, + // }) + // + // test.That(t, err, test.ShouldBeNil) } func TestMultiplePieces(t *testing.T) { @@ -2646,7 +2652,7 @@ func TestGetTransientDetections(t *testing.T) { testFn := func(t *testing.T, tc testCase) { t.Helper() - transformedGeoms, err := mr.getTransientDetections(ctx, injectedVis, camera.Named("test-camera"), tc.f) + 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) diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 6a5518529b8..b55ba85f224 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -58,7 +58,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 +66,7 @@ type moveRequest struct { obstacleDetectors map[vision.Service][]resource.Name replanCostFactor float64 fsService framesystem.Service + components map[resource.Name]resource.Resource executeBackgroundWorkers *sync.WaitGroup responseChan chan moveResponse @@ -97,7 +97,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 } @@ -173,7 +173,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", @@ -187,17 +186,30 @@ func (mr *moveRequest) getTransientDetections( } cameraOrigin := referenceframe.NewPoseInFrame(camName.ShortName(), spatialmath.NewZeroPose()) - cameraToBase, err := mr.fsService.TransformPose(ctx, cameraOrigin, mr.kinematicBase.Name().ShortName(), nil) + cameraToWorld, err := mr.fsService.TransformPose(ctx, cameraOrigin, mr.kinematicBase.Name().ShortName(), 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(), ) - cameraToBase = cameraOrigin + cameraToWorld = cameraOrigin } + // mr.planRequest.FrameSystem.Transform() + // positions map[string][]referenceframe.Input, + // object referenceframe.Transformable, + // dst string + // here we want to be using the framesystem to place the detection into the world frame + // to do this we will need to know what the current inputs are for the custom frame system we have built + // this is annoying and i think that i should just make a helper functipn for this? + + // currentInputs, _, err := mr.fsService.CurrentInputs(ctx) + // if err != nil { + // 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 @@ -206,15 +218,10 @@ func (mr *moveRequest) getTransientDetections( label += "_" + geometry.Label() } geometry.SetLabel(label) - - // transform the geometry to be relative to the base frame which is +Y forwards - relativeGeom := geometry.Transform(cameraToBase.Pose()) - - // apply any transformation on the geometry defined a priori by the caller - transformedGeom := relativeGeom.Transform(transformBy) - transformedGeoms = append(transformedGeoms, transformedGeom) + worldGeom := geometry.Transform(cameraToWorld.Pose()) + transientGeoms = append(transientGeoms, worldGeom) } - 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 @@ -237,23 +244,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 + continue } // construct new worldstate @@ -753,7 +754,6 @@ func (ms *builtIn) createBaseMoveRequest( WorldState: worldState, Options: valExtra.extra, }, - poseOrigin: startPose, kinematicBase: kb, replanCostFactor: valExtra.replanCostFactor, obstacleDetectors: obstacleDetectors, From 6a49f8ac9b29890b350398a54e1bbd9764186cda Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 10 Apr 2024 15:04:22 -0400 Subject: [PATCH 005/126] have fs place objects into worldframe within getTransientDetections --- components/base/kinematicbase/execution.go | 4 +- .../base/kinematicbase/fake_kinematics.go | 70 +++++++++----- components/base/kinematicbase/kinematics.go | 6 +- .../base/kinematicbase/ptgKinematics.go | 29 ++++-- motionplan/solverFrame.go | 12 +-- referenceframe/model.go | 7 +- services/motion/builtin/move_request.go | 92 +++++++++++-------- 7 files changed, 130 insertions(+), 90 deletions(-) diff --git a/components/base/kinematicbase/execution.go b/components/base/kinematicbase/execution.go index 6e4456ef412..0c3f2f92376 100644 --- a/components/base/kinematicbase/execution.go +++ b/components/base/kinematicbase/execution.go @@ -308,13 +308,13 @@ func (ptgk *ptgBaseKinematics) courseCorrect( if err != nil { return nil, err } - trajPose, err := ptgk.frame.Transform(currentInputs) + trajPose, err := ptgk.planningFrame.Transform(currentInputs) if err != nil { return nil, err } // Note that the arcSegment start position corresponds to the start configuration, which may not be at pose distance 0, so we must // account for this. - trajStartPose, err := ptgk.frame.Transform(arcSteps[arcIdx].arcSegment.StartConfiguration) + trajStartPose, err := ptgk.planningFrame.Transform(arcSteps[arcIdx].arcSegment.StartConfiguration) if err != nil { return nil, err } diff --git a/components/base/kinematicbase/fake_kinematics.go b/components/base/kinematicbase/fake_kinematics.go index 6f3b8840941..680fad53f91 100644 --- a/components/base/kinematicbase/fake_kinematics.go +++ b/components/base/kinematicbase/fake_kinematics.go @@ -79,6 +79,10 @@ func (fk *fakeDiffDriveKinematics) Kinematics() referenceframe.Frame { return fk.planningFrame } +func (fk *fakeDiffDriveKinematics) ExecutionFrame() referenceframe.Frame { + return fk.executionFrame +} + func (fk *fakeDiffDriveKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { fk.lock.RLock() defer fk.lock.RUnlock() @@ -118,17 +122,17 @@ func (fk *fakeDiffDriveKinematics) CurrentPosition(ctx context.Context) (*refere type fakePTGKinematics struct { *fake.Base - localizer motion.Localizer - frame referenceframe.Frame - options Options - sensorNoise spatialmath.Pose - ptgs []tpspace.PTGSolver - currentInput []referenceframe.Input - origin *referenceframe.PoseInFrame - positionlock sync.RWMutex - inputLock sync.RWMutex - logger logging.Logger - sleepTime int + localizer motion.Localizer + planningFrame, executionFrame referenceframe.Frame + options Options + sensorNoise spatialmath.Pose + ptgs []tpspace.PTGSolver + currentInput []referenceframe.Input + origin *referenceframe.PoseInFrame + positionlock sync.RWMutex + inputLock sync.RWMutex + logger logging.Logger + sleepTime int } // WrapWithFakePTGKinematics creates a PTG KinematicBase from the fake Base so that it satisfies the ModelFramer and InputEnabled @@ -174,7 +178,8 @@ func WrapWithFakePTGKinematics( nonzeroBaseTurningRadiusMeters := (baseMillimetersPerSecond / rdkutils.DegToRad(angVelocityDegsPerSecond)) / 1000. - frame, err := tpspace.NewPTGFrameFromKinematicOptions( + // construct planning frame + planningFrame, err := tpspace.NewPTGFrameFromKinematicOptions( b.Name().ShortName(), logger, nonzeroBaseTurningRadiusMeters, @@ -187,25 +192,36 @@ func WrapWithFakePTGKinematics( return nil, err } + // construct execution frame + executionFrame, err := referenceframe.New2DMobileModelFrame( + b.Name().ShortName()+"ExecutionFrame", + planningFrame.DoF(), + geometries[0], + ) + if err != nil { + return nil, err + } + if sensorNoise == nil { sensorNoise = spatialmath.NewZeroPose() } - ptgProv, ok := frame.(tpspace.PTGProvider) + ptgProv, ok := planningFrame.(tpspace.PTGProvider) if !ok { return nil, errors.New("unable to cast ptgk frame to a PTG Provider") } ptgs := ptgProv.PTGSolvers() fk := &fakePTGKinematics{ - Base: b, - frame: frame, - origin: origin, - ptgs: ptgs, - currentInput: zeroInput, - sensorNoise: sensorNoise, - logger: logger, - sleepTime: sleepTime, + Base: b, + planningFrame: planningFrame, + executionFrame: executionFrame, + origin: origin, + ptgs: ptgs, + currentInput: zeroInput, + sensorNoise: sensorNoise, + logger: logger, + sleepTime: sleepTime, } initLocalizer := &fakePTGKinematicsLocalizer{fk} fk.localizer = motion.TwoDLocalizer(initLocalizer) @@ -215,7 +231,11 @@ func WrapWithFakePTGKinematics( } func (fk *fakePTGKinematics) Kinematics() referenceframe.Frame { - return fk.frame + return fk.planningFrame +} + +func (fk *fakePTGKinematics) ExecutionFrame() referenceframe.Frame { + return fk.executionFrame } func (fk *fakePTGKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { @@ -240,7 +260,7 @@ func (fk *fakePTGKinematics) GoToInputs(ctx context.Context, inputSteps ...[]ref fk.currentInput = []referenceframe.Input{inputs[0], inputs[1], {Value: 0}} fk.inputLock.Unlock() - finalPose, err := fk.frame.Transform(inputs) + finalPose, err := fk.planningFrame.Transform(inputs) if err != nil { return err } @@ -250,7 +270,7 @@ func (fk *fakePTGKinematics) GoToInputs(ctx context.Context, inputSteps ...[]ref var interpolatedConfigurations [][]referenceframe.Input for i := 0; i <= steps; i++ { interp := float64(i) / float64(steps) - interpConfig, err := fk.frame.Interpolate(startCfg, inputs, interp) + interpConfig, err := fk.planningFrame.Interpolate(startCfg, inputs, interp) if err != nil { return err } @@ -260,7 +280,7 @@ func (fk *fakePTGKinematics) GoToInputs(ctx context.Context, inputSteps ...[]ref if ctx.Err() != nil { return ctx.Err() } - relativePose, err := fk.frame.Transform(inter) + relativePose, err := fk.planningFrame.Transform(inter) if err != nil { return err } diff --git a/components/base/kinematicbase/kinematics.go b/components/base/kinematicbase/kinematics.go index b240b4bde81..1649317a147 100644 --- a/components/base/kinematicbase/kinematics.go +++ b/components/base/kinematicbase/kinematics.go @@ -22,9 +22,9 @@ type KinematicBase interface { motion.Localizer referenceframe.InputEnabled - Kinematics() referenceframe.Frame // this should be remaned to PlanningFrame()? - // ExecutionFrame() referenceframe.Frame - // we should add ExecutionFrame() as a method on this interface + Kinematics() referenceframe.Frame + ExecutionFrame() referenceframe.Frame + // ErrorState takes a complete motionplan, as well as the index of the currently-executing set of inputs, and computes the pose // difference between where the robot in fact is, and where it ought to be. ErrorState(context.Context) (spatialmath.Pose, error) diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index be4c002db04..a2c465e3b84 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -30,7 +30,7 @@ type ptgBaseKinematics struct { base.Base motion.Localizer logger logging.Logger - frame referenceframe.Frame + planningFrame, executionFrame referenceframe.Frame ptgs []tpspace.PTGSolver courseCorrectionIdx int linVelocityMMPerSecond float64 @@ -99,7 +99,7 @@ func wrapWithPTGKinematics( } nonzeroBaseTurningRadiusMeters := (linVelocityMMPerSecond / rdkutils.DegToRad(angVelocityDegsPerSecond)) / 1000. - frame, err := tpspace.NewPTGFrameFromKinematicOptions( + planningFrame, err := tpspace.NewPTGFrameFromKinematicOptions( b.Name().ShortName(), logger, nonzeroBaseTurningRadiusMeters, @@ -111,14 +111,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 } @@ -132,11 +132,22 @@ func wrapWithPTGKinematics( origin = originPIF.Pose() } + // construct executionFrame + executionFrame, err := referenceframe.New2DMobileModelFrame( + b.Name().ShortName()+"ExecutionFrame", + planningFrame.DoF(), + geometries[0], + ) + if err != nil { + return nil, err + } + return &ptgBaseKinematics{ Base: b, Localizer: localizer, logger: logger, - frame: frame, + planningFrame: planningFrame, + executionFrame: executionFrame, ptgs: ptgs, courseCorrectionIdx: courseCorrectionIdx, linVelocityMMPerSecond: linVelocityMMPerSecond, @@ -149,7 +160,11 @@ func wrapWithPTGKinematics( } func (ptgk *ptgBaseKinematics) Kinematics() referenceframe.Frame { - return ptgk.frame + return ptgk.planningFrame +} + +func (ptgk *ptgBaseKinematics) ExecutionFrame() referenceframe.Frame { + return ptgk.executionFrame } func (ptgk *ptgBaseKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { @@ -179,7 +194,7 @@ func (ptgk *ptgBaseKinematics) ErrorState(ctx context.Context) (spatialmath.Pose return nil, err } - currPoseInArc, err := ptgk.frame.Transform(currentInputs) + currPoseInArc, err := ptgk.planningFrame.Transform(currentInputs) if err != nil { return nil, err } diff --git a/motionplan/solverFrame.go b/motionplan/solverFrame.go index 8f9b5c18fa5..760c8ec1ddb 100644 --- a/motionplan/solverFrame.go +++ b/motionplan/solverFrame.go @@ -3,6 +3,7 @@ package motionplan import ( "errors" "fmt" + "strings" "go.uber.org/multierr" pb "go.viam.com/api/component/arm/v1" @@ -271,14 +272,11 @@ func (sf *solverFrame) Geometries(inputs []frame.Input) (*frame.GeometriesInFram inputMap := sf.sliceToMap(inputs) sfGeometries := []spatial.Geometry{} - // i think this function will need to be changed - // if we are relative we know so - // we then assume that sf.name is the name of the kinematic base and anything above it should be - // treated as a static transform frame - - // all other frame, we do not need to worry about and they should be treated as normal - for _, fName := range sf.movingFS.FrameNames() { + // if we have an execution frame no need to transform it + if strings.Contains(fName, "ExecutionFrame") { + continue + } f := sf.fss.Frame(fName) if f == nil { return nil, frame.NewFrameMissingError(fName) diff --git a/referenceframe/model.go b/referenceframe/model.go index 2b5a6b49350..f6395ca9514 100644 --- a/referenceframe/model.go +++ b/referenceframe/model.go @@ -72,12 +72,6 @@ func (m *SimpleModel) Transform(inputs []Input) (spatialmath.Pose, error) { if err != nil && frames == nil { return nil, err } - // this confuses me a lot - // why not just - // return spatialmath.NewPose( - // r3.Vector{X: inputs[0].Value, Y: inputs[1].Value, Z: 0}, - // &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: inputs[2].Value}, - // ), nil return frames[0].transform, err } @@ -130,6 +124,7 @@ func (m *SimpleModel) ProtobufFromInput(input []Input) *pb.JointPositions { // Geometries returns an object representing the 3D space associeted with the staticFrame. func (m *SimpleModel) Geometries(inputs []Input) (*GeometriesInFrame, error) { + // should I add a check here to just return nothing if we have no geoms? frames, err := m.inputsToFrames(inputs, true) if err != nil && frames == nil { return nil, err diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index b55ba85f224..9ff6fbefa59 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -66,7 +66,7 @@ type moveRequest struct { obstacleDetectors map[vision.Service][]resource.Name replanCostFactor float64 fsService framesystem.Service - components map[resource.Name]resource.Resource + absoluteFS referenceframe.FrameSystem executeBackgroundWorkers *sync.WaitGroup responseChan chan moveResponse @@ -180,34 +180,16 @@ func (mr *moveRequest) getTransientDetections( camName.ShortName(), ) - detections, err := visSrvc.GetObjectPointClouds(ctx, camName.Name, nil) + currentInputs, _, err := mr.getCurrentInputsAndPosition(ctx) if err != nil { return nil, err } - cameraOrigin := referenceframe.NewPoseInFrame(camName.ShortName(), spatialmath.NewZeroPose()) - cameraToWorld, 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(), - ) - cameraToWorld = cameraOrigin + return nil, err } - // mr.planRequest.FrameSystem.Transform() - // positions map[string][]referenceframe.Input, - // object referenceframe.Transformable, - // dst string - // here we want to be using the framesystem to place the detection into the world frame - // to do this we will need to know what the current inputs are for the custom frame system we have built - // this is annoying and i think that i should just make a helper functipn for this? - - // currentInputs, _, err := mr.fsService.CurrentInputs(ctx) - // if err != nil { - // return nil, err - // } - // transformed detections transientGeoms := []spatialmath.Geometry{} for i, detection := range detections { @@ -218,8 +200,22 @@ func (mr *moveRequest) getTransientDetections( label += "_" + geometry.Label() } geometry.SetLabel(label) - worldGeom := geometry.Transform(cameraToWorld.Pose()) - transientGeoms = append(transientGeoms, worldGeom) + + tf, err := mr.absoluteFS.Transform( + currentInputs, + 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, transientGeoms), nil } @@ -263,23 +259,11 @@ func (mr *moveRequest) obstaclesIntersectPlan( return state.ExecuteResponse{}, err } - // build representation of frame system's inputs - currentInputs, err := mr.kinematicBase.CurrentInputs(ctx) + // build representation of frame system's inputs and get the current position of the base + inputMap, currentPosition, err := mr.getCurrentInputsAndPosition(ctx) if err != nil { return state.ExecuteResponse{}, err } - inputMap := referenceframe.StartPositions(mr.planRequest.FrameSystem) - inputMap[mr.kinematicBase.Name().ShortName()] = currentInputs - - // get the current position of the base - currentPosition, err := mr.kinematicBase.CurrentPosition(ctx) - if err != nil { - return state.ExecuteResponse{}, err - } - - // Note: the value of wayPointIndex is subject to change between when this function is first entered - // versus when CheckPlan is actually called. - // We load the wayPointIndex value to ensure that all information is up to date. // get the pose difference between where the robot is versus where it ought to be. errorState, err := mr.kinematicBase.ErrorState(ctx) @@ -289,7 +273,7 @@ func (mr *moveRequest) obstaclesIntersectPlan( mr.logger.CDebugf(ctx, "CheckPlan inputs: \n currentPosition: %v\n currentInputs: %v\n errorState: %v\n worldstate: %s", spatialmath.PoseToProtobuf(currentPosition.Pose()), - currentInputs, + inputMap, spatialmath.PoseToProtobuf(errorState), worldState.String(), ) @@ -301,7 +285,7 @@ func (mr *moveRequest) obstaclesIntersectPlan( // plan, // waypointIndex, // worldState, // detected obstacles by this instance of camera + service - // mr.planRequest.FrameSystem, + // mr.absoluteFS // currentPosition.Pose(), // currentPosition of robot accounts for errorState // inputMap, // errorState, // deviation of robot from plan @@ -316,6 +300,22 @@ func (mr *moveRequest) obstaclesIntersectPlan( return state.ExecuteResponse{}, nil } +func (mr *moveRequest) getCurrentInputsAndPosition(ctx context.Context) (map[string][]referenceframe.Input, *referenceframe.PoseInFrame, error) { + currentInputs := referenceframe.StartPositions(mr.absoluteFS) + pif, err := mr.kinematicBase.CurrentPosition(ctx) + if err != nil { + return nil, nil, err + } + currentInputs[mr.kinematicBase.Name().ShortName()+"ExecutionFrame"] = referenceframe.FloatsToInputs( + []float64{ + pif.Pose().Point().X, + pif.Pose().Point().Y, + pif.Pose().Orientation().OrientationVectorRadians().Theta, + }, + ) + return currentInputs, pif, nil +} + func kbOptionsFromCfg(motionCfg *validatedMotionConfiguration, validatedExtra validatedExtra) kinematicbase.Options { kinematicsOptions := kinematicbase.NewKinematicBaseOptions() @@ -676,6 +676,17 @@ func (ms *builtIn) createBaseMoveRequest( return nil, err } + // create checking fs + collisionFS := referenceframe.NewEmptyFrameSystem("collisionFS") + err = collisionFS.AddFrame(kb.ExecutionFrame(), collisionFS.World()) + if err != nil { + return nil, err + } + err = collisionFS.MergeFrameSystem(baseOnlyFS, kb.ExecutionFrame()) + if err != nil { + return nil, err + } + startPoseIF, err := kb.CurrentPosition(ctx) if err != nil { return nil, err @@ -758,6 +769,7 @@ func (ms *builtIn) createBaseMoveRequest( replanCostFactor: valExtra.replanCostFactor, obstacleDetectors: obstacleDetectors, fsService: ms.fsService, + absoluteFS: collisionFS, executeBackgroundWorkers: &backgroundWorkers, From 44b3287a38fdaa6aa00a7fdcecad127fc1f94628 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 19 Apr 2024 15:25:51 -0400 Subject: [PATCH 006/126] clean up + note about how to interpolate --- motionplan/motionPlanner.go | 7 ++++ motionplan/solverFrame.go | 75 ------------------------------------- 2 files changed, 7 insertions(+), 75 deletions(-) diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 574a55c368f..567a3e051df 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -496,6 +496,13 @@ func CheckPlan( // create a list of segments to iterate through // doesn't this need to also happen for absolute plans?? segments := make([]*ik.Segment, 0, len(poses)-wayPointIdx) + // current inputs for the execution frame would be [x, y, theta] + // current inputs for the planning frame would be [i, alpha, di, df] + // want interpolations to be + // [x, y, theta, 0, 0, 0, 0], + // [x, y, theta, i, alpha, di, di + eplison] + // ... + // [x, y, thetha, i, alpha, df, df]??? if relative { // get the inputs we were partway through executing checkFrameGoalInputs, err := sf.mapToSlice(plan.Trajectory()[wayPointIdx]) diff --git a/motionplan/solverFrame.go b/motionplan/solverFrame.go index 760c8ec1ddb..40af33f629c 100644 --- a/motionplan/solverFrame.go +++ b/motionplan/solverFrame.go @@ -273,7 +273,6 @@ func (sf *solverFrame) Geometries(inputs []frame.Input) (*frame.GeometriesInFram sfGeometries := []spatial.Geometry{} for _, fName := range sf.movingFS.FrameNames() { - // if we have an execution frame no need to transform it if strings.Contains(fName, "ExecutionFrame") { continue } @@ -286,80 +285,6 @@ func (sf *solverFrame) Geometries(inputs []frame.Input) (*frame.GeometriesInFram return nil, err } gf, err := f.Geometries(inputs) - fmt.Println("about to print f.Geometries(inputs)") - for _, g := range gf.Geometries() { - fmt.Println("g.Label(): ", g.Label()) - } - if gf == nil { - // only propagate errors that result in nil geometry - multierr.AppendInto(&errAll, err) - continue - } - var tf frame.Transformable - tf, err = sf.fss.Transform(inputMap, gf, frame.World) - if err != nil { - return nil, err - } - sfGeometries = append(sfGeometries, tf.(*frame.GeometriesInFrame).Geometries()...) - } - return frame.NewGeometriesInFrame(frame.World, sfGeometries), errAll -} - -func (sf *solverFrame) MYGeometries(inputs []frame.Input) (*frame.GeometriesInFrame, error) { - if len(inputs) != len(sf.DoF()) { - return nil, frame.NewIncorrectInputLengthError(len(inputs), len(sf.DoF())) - } - var errAll error - inputMap := sf.sliceToMap(inputs) - sfGeometries := []spatial.Geometry{} - - // first we check if this is a solverFrame which with we need to concern ourselves with for Ptgs - isRelative := len(sf.ptgs) > 0 - if isRelative { - solveFrameList, err := sf.fss.TracebackFrame(sf.solveFrame) - if err != nil { - return nil, err - } - // we can probably just explicitly check the length of the list here - // it should only be wrld -- executionFrame -- planningFrame - // further, I believe that the indicies at which these frames are found shall remain constant - // hence, we can simplifify our code here - // todo: need to figure out the inidicied which are proper - for _, f := range solveFrameList { - inputs, err := frame.GetFrameInputs(f, inputMap) - if err != nil { - return nil, err - } - gf, err := f.Geometries(inputs) - if gf == nil { - // only propagate errors that result in nil geometry - multierr.AppendInto(&errAll, err) - continue - } - } - } - - // i think this function will need to be changed - // if we are relative we know so - // we then assume that sf.name is the name of the kinematic base and anything above it should be - // treated as a static transform frame - - // all other frame, we do not need to worry about and they should be treated as normal - - for _, fName := range sf.movingFS.FrameNames() { - f := sf.fss.Frame(fName) - if f == nil { - return nil, frame.NewFrameMissingError(fName) - } - inputs, err := frame.GetFrameInputs(f, inputMap) - if err != nil { - return nil, err - } - gf, err := f.Geometries(inputs) - fmt.Println("about to print f.Geometries(inputs)") - for _, g := range gf.Geometries() { - fmt.Println("g.Label(): ", g.Label()) - } if gf == nil { // only propagate errors that result in nil geometry multierr.AppendInto(&errAll, err) From ba2be7dcfdd8ba2707b74219b7f76fcf4ad53dec Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Tue, 30 Apr 2024 11:58:35 -0400 Subject: [PATCH 007/126] remove '&' from orientation printout when doing plan.Path.String() --- motionplan/plan.go | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motionplan/plan.go b/motionplan/plan.go index 85aa6b993a4..ca5e591b5aa 100644 --- a/motionplan/plan.go +++ b/motionplan/plan.go @@ -172,7 +172,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 From 526b14d95c0e2d88237c9adb628dd1116505c03a Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 1 May 2024 12:00:52 -0400 Subject: [PATCH 008/126] add ExecutionFrame function impl + correct references to transforming off the planningFrame within execution.go --- components/base/kinematicbase/execution.go | 2 +- components/base/kinematicbase/ptgKinematics.go | 8 ++++++-- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/components/base/kinematicbase/execution.go b/components/base/kinematicbase/execution.go index 42a468a3837..3f9e20b6cc5 100644 --- a/components/base/kinematicbase/execution.go +++ b/components/base/kinematicbase/execution.go @@ -322,7 +322,7 @@ func (ptgk *ptgBaseKinematics) courseCorrect( arcSteps[arcIdx].arcSegment.StartConfiguration[startDistanceAlongTrajectoryIndex], currentInputs[startDistanceAlongTrajectoryIndex], } - trajPose, err := ptgk.frame.Transform(execInputs) + trajPose, err := ptgk.planningFrame.Transform(execInputs) if err != nil { return nil, err } diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index 64d8e1166e0..2ddd98e8b5c 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -161,7 +161,11 @@ func wrapWithPTGKinematics( } func (ptgk *ptgBaseKinematics) Kinematics() referenceframe.Frame { - return ptgk.frame + return ptgk.planningFrame +} + +func (ptgk *ptgBaseKinematics) ExecutionFrame() referenceframe.Frame { + return ptgk.executionFrame } // For a ptgBaseKinematics, `CurrentInputs` returns inputs which reflect what the base is currently doing. @@ -204,7 +208,7 @@ func (ptgk *ptgBaseKinematics) ErrorState(ctx context.Context) (spatialmath.Pose currentInputs[startDistanceAlongTrajectoryIndex], } - currPoseInArc, err := ptgk.frame.Transform(executedInputs) + currPoseInArc, err := ptgk.planningFrame.Transform(executedInputs) if err != nil { return nil, err } From 656d154343ccb91952287d6377f8dc3ee221fc96 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 1 May 2024 12:01:54 -0400 Subject: [PATCH 009/126] simplify how we create robot collision entities to only use frameInputs and not transform off a pose if we are relative --- motionplan/planManager.go | 18 ------------------ 1 file changed, 18 deletions(-) diff --git a/motionplan/planManager.go b/motionplan/planManager.go index ab21bdc5cb7..a7cfed73125 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -505,24 +505,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 - } - fmt.Println("PRINTING movingRobotGeometries") - for _, g := range movingRobotGeometries { - fmt.Println("g.Label(): ", g.Label()) - fmt.Println("g.Pose(): ", spatialmath.PoseToProtobuf(g.Pose())) - fmt.Println("-----------------------") - } // find all geometries that are not moving but are in the frame system staticRobotGeometries := make([]spatialmath.Geometry, 0) From 09c27a8d48df6d04698c1eebcc497ede7524577d Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 1 May 2024 12:02:42 -0400 Subject: [PATCH 010/126] fulfill placing obstacles into the world frame using only the newly introduced absolute frame system which houses the execution frame + planning frame --- services/motion/builtin/move_request.go | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 9ff6fbefa59..7eba35d3b6e 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -259,11 +259,17 @@ func (mr *moveRequest) obstaclesIntersectPlan( return state.ExecuteResponse{}, err } - // build representation of frame system's inputs and get the current position of the base + // build representation of frame system's inputs inputMap, currentPosition, err := mr.getCurrentInputsAndPosition(ctx) if err != nil { return state.ExecuteResponse{}, err } + // update inputs for the kinematic bases' planning frame + planningFrameCurrentInputs, err := mr.kinematicBase.CurrentInputs(ctx) + if err != nil { + return state.ExecuteResponse{}, err + } + inputMap[mr.kinematicBase.Name().ShortName()] = planningFrameCurrentInputs // get the pose difference between where the robot is versus where it ought to be. errorState, err := mr.kinematicBase.ErrorState(ctx) From a8eca26eb82d4e787054c018e26ce7c92f8eaf3f Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 1 May 2024 12:05:59 -0400 Subject: [PATCH 011/126] clean up remove print statements --- referenceframe/model.go | 3 --- 1 file changed, 3 deletions(-) diff --git a/referenceframe/model.go b/referenceframe/model.go index f6395ca9514..431d9acb2b3 100644 --- a/referenceframe/model.go +++ b/referenceframe/model.go @@ -3,7 +3,6 @@ package referenceframe import ( "encoding/binary" "encoding/json" - "fmt" "math" "math/rand" "strings" @@ -124,7 +123,6 @@ func (m *SimpleModel) ProtobufFromInput(input []Input) *pb.JointPositions { // Geometries returns an object representing the 3D space associeted with the staticFrame. func (m *SimpleModel) Geometries(inputs []Input) (*GeometriesInFrame, error) { - // should I add a check here to just return nothing if we have no geoms? frames, err := m.inputsToFrames(inputs, true) if err != nil && frames == nil { return nil, err @@ -143,7 +141,6 @@ func (m *SimpleModel) Geometries(inputs []Input) (*GeometriesInFrame, error) { geometries = append(geometries, placedGeom) } } - fmt.Println("geometries[0].Label(): ", geometries[0].Label()) return NewGeometriesInFrame(m.name, geometries), errAll } From 61d8fc12d8b1789d4551192ce43ec6fffb1d9b67 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 1 May 2024 12:06:06 -0400 Subject: [PATCH 012/126] clean up remove print statements --- components/base/fake/fake.go | 2 -- 1 file changed, 2 deletions(-) diff --git a/components/base/fake/fake.go b/components/base/fake/fake.go index 69aa03142a4..076f6f7f59c 100644 --- a/components/base/fake/fake.go +++ b/components/base/fake/fake.go @@ -3,7 +3,6 @@ package fake import ( "context" - "fmt" "github.com/golang/geo/r3" @@ -51,7 +50,6 @@ func NewBase(_ context.Context, _ resource.Dependencies, conf resource.Config, l if err != nil { return nil, err } - fmt.Println("geometry.Label(): ", geometry.Label()) b.Geometry = []spatialmath.Geometry{geometry} } b.WidthMeters = defaultWidthMm * 0.001 From 5bc3a0a64654eeef4825c46b592ea5abe806b532 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 1 May 2024 12:06:22 -0400 Subject: [PATCH 013/126] clean up remove print statements --- motionplan/tpspace/ptgGroupFrame.go | 1 - 1 file changed, 1 deletion(-) diff --git a/motionplan/tpspace/ptgGroupFrame.go b/motionplan/tpspace/ptgGroupFrame.go index 23fad540c3e..e11ca42d129 100644 --- a/motionplan/tpspace/ptgGroupFrame.go +++ b/motionplan/tpspace/ptgGroupFrame.go @@ -231,7 +231,6 @@ func (pf *ptgGroupFrame) Geometries(inputs []referenceframe.Input) (*referencefr for _, geom := range pf.geometries { geoms = append(geoms, geom.Transform(transformedPose)) } - fmt.Println("geoms[0].Label(): ", geoms[0].Label()) return referenceframe.NewGeometriesInFrame(pf.name, geoms), nil } From 7c07b15f8308d1ba839b575f177f02c013a972bb Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 1 May 2024 12:07:16 -0400 Subject: [PATCH 014/126] remove print stmts --- motionplan/constraint.go | 4 ---- 1 file changed, 4 deletions(-) diff --git a/motionplan/constraint.go b/motionplan/constraint.go index bf10b2b76a5..16a363ccb51 100644 --- a/motionplan/constraint.go +++ b/motionplan/constraint.go @@ -335,10 +335,6 @@ func NewCollisionConstraint( return false } internalGeoms = internal.Geometries() - // for _, g := range internalGeoms { - // fmt.Println("g.String(): ", g.String()) - // fmt.Println("g.Pose(): ", spatial.PoseToProtobuf(g.Pose())) - // } case state.Position != nil: // If we didn't pass a Configuration, but we do have a Position, then get the geometries at the zero state and // transform them to the Position From 7185d6e333a432e97e3e1735b48d78a3736542d2 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 1 May 2024 12:08:15 -0400 Subject: [PATCH 015/126] remove note - clean up --- motionplan/planManager.go | 1 - 1 file changed, 1 deletion(-) diff --git a/motionplan/planManager.go b/motionplan/planManager.go index a7cfed73125..da6d19150fe 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -37,7 +37,6 @@ type planManager struct { activeBackgroundWorkers sync.WaitGroup useTPspace bool - // ugh, we might need to add another bool here??? } func newPlanManager( From d454d2145469b8f124b4ba61863c656f4e4d592e Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 1 May 2024 12:20:35 -0400 Subject: [PATCH 016/126] logic complete-ish, sans tests --- motionplan/motionPlanner.go | 30 +-- motionplan/solverFrame.go | 22 +- motionplan/tpSpaceRRT_test.go | 267 ++++++++++++++---------- services/motion/builtin/move_request.go | 6 +- 4 files changed, 184 insertions(+), 141 deletions(-) diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 8baa4c8f3ec..c8a9aa32577 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -17,6 +17,7 @@ import ( "go.viam.com/rdk/logging" "go.viam.com/rdk/motionplan/ik" + "go.viam.com/rdk/referenceframe" frame "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -449,10 +450,6 @@ func CheckPlan( return errors.New("plan must have at least one element") } - fmt.Println("have the following FrameSystem") - fmt.Println("fs.FrameNames(): ", fs.FrameNames()) - fmt.Println("currentInputs: ", currentInputs) - // construct solverFrame // Note that this requires all frames which move as part of the plan, to have an // entry in the very first plan waypoint @@ -494,15 +491,7 @@ func CheckPlan( } // create a list of segments to iterate through - // doesn't this need to also happen for absolute plans?? segments := make([]*ik.Segment, 0, len(poses)-wayPointIdx) - // current inputs for the execution frame would be [x, y, theta] - // current inputs for the planning frame would be [i, alpha, di, df] - // want interpolations to be - // [x, y, theta, 0, 0, 0, 0], - // [x, y, theta, i, alpha, di, di + eplison] - // ... - // [x, y, thetha, i, alpha, df, df]??? if relative { // get checkFrame's currentInputs // *currently* it is guaranteed that a relative frame will constitute 100% of a solver frame's dof @@ -513,7 +502,7 @@ func CheckPlan( // pre-pend to segments so we can connect to the input we have not finished actuating yet segments = append(segments, &ik.Segment{ - StartPosition: startPose, + StartPosition: currentPose, EndPosition: poses[wayPointIdx], StartConfiguration: checkFrameCurrentInputs, EndConfiguration: checkFrameCurrentInputs, @@ -526,6 +515,7 @@ func CheckPlan( currPose, nextPose spatialmath.Pose, currInput, nextInput map[string][]frame.Input, ) (*ik.Segment, error) { + // there will be an issue here since currInputs is not of the correct length currInputSlice, err := sf.mapToSlice(currInput) if err != nil { return nil, err @@ -550,7 +540,18 @@ func CheckPlan( // iterate through remaining plan and append remaining segments to check for i := wayPointIdx; i < len(offsetPlan.Path())-1; i++ { - segment, err := createSegment(poses[i], poses[i+1], offsetPlan.Trajectory()[i], offsetPlan.Trajectory()[i+1]) + // do we want to add a relative special case here or in the anon function + currInput := offsetPlan.Trajectory()[i] + nextInput := offsetPlan.Trajectory()[i+1] + if relative { + currInput[checkFrame.Name()+"ExecutionFrame"] = referenceframe.FloatsToInputs( + []float64{poses[i].Point().X, poses[i].Point().Y, poses[i].Orientation().OrientationVectorRadians().Theta}, + ) + nextInput[checkFrame.Name()+"ExecutionFrame"] = referenceframe.FloatsToInputs( + []float64{poses[i+1].Point().X, poses[i+1].Point().Y, poses[i+1].Orientation().OrientationVectorRadians().Theta}, + ) + } + segment, err := createSegment(poses[i], poses[i+1], currInput, nextInput) if err != nil { return err } @@ -571,7 +572,6 @@ func CheckPlan( if err != nil { return err } - fmt.Println("poseInPath: ", spatialmath.PoseToProtobuf(poseInPath)) // Check if look ahead distance has been reached currentTravelDistanceMM := totalTravelDistanceMM + poseInPath.Point().Distance(segment.StartPosition.Point()) diff --git a/motionplan/solverFrame.go b/motionplan/solverFrame.go index 69ef5fef0b4..8dfd3c89e28 100644 --- a/motionplan/solverFrame.go +++ b/motionplan/solverFrame.go @@ -2,7 +2,6 @@ package motionplan import ( "errors" - "fmt" "strings" "go.uber.org/multierr" @@ -44,8 +43,6 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, return nil, err } - // is the solution that frames which are above the solveFrameName should be not worried about? - // get solve frame solveFrame := fs.Frame(solveFrameName) if solveFrame == nil { @@ -55,10 +52,6 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, if err != nil { return nil, err } - fmt.Println("printing solveFrameList", solveFrameList) - for _, f := range solveFrameList { - fmt.Println("f.Name(): ", f.Name()) - } if len(solveFrameList) == 0 { return nil, errors.New("solveFrameList was empty") } @@ -86,7 +79,6 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, if err != nil { return nil, err } - fmt.Println("pivotFrame: ", pivotFrame.Name()) if pivotFrame.Name() == frame.World { frames = uniqInPlaceSlice(append(solveFrameList, goalFrameList...)) moving, err = movingFS(solveFrameList) @@ -220,13 +212,19 @@ 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 { + // if we are dealing with the execution frame, no need to interpolate, just return what we got + dof := len(currFrame.DoF()) + posIdx fromSubset := from[posIdx:dof] toSubset := to[posIdx:dof] posIdx = dof - - interpSub, err := frame.Interpolate(fromSubset, toSubset, by) + var interpSub []frame.Input + var err error + if strings.Contains(currFrame.Name(), "ExecutionFrame") { + interp = append(interp, from...) + continue + } + 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 91c509a20f7..acc1053388d 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -4,6 +4,7 @@ package motionplan import ( "context" + "fmt" "math" "math/rand" "testing" @@ -281,123 +282,171 @@ func TestPtgCheckPlan(t *testing.T) { test.That(t, err, test.ShouldBeNil) plan, err := newRRTPlan(nodes, sf, true) test.That(t, err, test.ShouldBeNil) + fmt.Println(plan.Path()) + fmt.Println(plan.Trajectory()) - startPose := spatialmath.NewPoseFromPoint(r3.Vector{0, 0, 0}) - errorState := startPose - inputs := plan.Trajectory()[0] - - t.Run("base case - validate plan without obstacles", func(t *testing.T) { - err := CheckPlan(ackermanFrame, plan, 0, nil, fs, startPose, inputs, errorState, 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 = CheckPlan(ackermanFrame, plan, 0, worldState, fs, startPose, inputs, errorState, math.Inf(1), logger) - test.That(t, err, test.ShouldNotBeNil) - }) + steps, err := plan.Trajectory().GetFrameInputs("ackframe") + test.That(t, err, test.ShouldBeNil) + firstNonZeroInput := steps[1] + myCustomInput := []referenceframe.Input{ + firstNonZeroInput[0], firstNonZeroInput[1], {Value: 2000}, {Value: 2000}, + } + myCustomEplisonInput := []referenceframe.Input{ + firstNonZeroInput[0], firstNonZeroInput[1], {Value: 2000}, {Value: 2005}, + } - // create camera_origin frame - cameraOriginFrame, err := referenceframe.NewStaticFrame("camera-origin", spatialmath.NewPoseFromPoint(r3.Vector{0, -20, 0})) + pose1, err := ackermanFrame.Transform(myCustomInput) test.That(t, err, test.ShouldBeNil) - err = fs.AddFrame(cameraOriginFrame, ackermanFrame) + pose2, err := ackermanFrame.Transform(myCustomEplisonInput) test.That(t, err, test.ShouldBeNil) - // create camera geometry - cameraGeom, err := spatialmath.NewBox( - spatialmath.NewZeroPose(), - r3.Vector{1, 1, 1}, "camera", - ) + fmt.Println("pose1: ", spatialmath.PoseToProtobuf(pose1)) + fmt.Println("pose2: ", spatialmath.PoseToProtobuf(pose2)) + + myOtherCustomInput := []referenceframe.Input{ + firstNonZeroInput[0], firstNonZeroInput[1], {Value: 2000}, firstNonZeroInput[3], + } + pose3, err := ackermanFrame.Transform(myCustomEplisonInput) 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, - ) + var pif *referenceframe.PoseInFrame + for _, v := range plan.Path()[1] { + pif = v + } + fmt.Println("pif: ", spatialmath.PoseToProtobuf(pif.Pose())) + + customSegment := &ik.Segment{ + Frame: ackermanFrame, + StartPosition: pose3, + StartConfiguration: myOtherCustomInput, + EndConfiguration: myOtherCustomInput, + EndPosition: pif.Pose(), + } + + interpolations, err := interpolateSegment(customSegment, 2) 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) - - err = CheckPlan(ackermanFrame, plan, 1, worldState, fs, startPose, inputs, errorState, 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 = CheckPlan(ackermanFrame, plan, 1, worldState, fs, startPose, inputs, errorState, 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) - - err = CheckPlan(ackermanFrame, plan, 2, worldState, fs, startPose, inputs, errorState, 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()) - errorState := spatialmath.PoseDelta(pathPose, startPose) - - err = CheckPlan(ackermanFrame, plan, 2, worldState, fs, startPose, inputs, errorState, math.Inf(1), logger) - test.That(t, err, test.ShouldBeNil) - }) + for _, i := range interpolations { + fmt.Println("i: ", i) + } + + // ackermanFrame.Transform() + + // startPose := spatialmath.NewPoseFromPoint(r3.Vector{0, 0, 0}) + // errorState := startPose + // inputs := plan.Trajectory()[0] + + // t.Run("base case - validate plan without obstacles", func(t *testing.T) { + // err := CheckPlan(ackermanFrame, plan, 0, nil, fs, startPose, inputs, errorState, 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 = CheckPlan(ackermanFrame, plan, 0, worldState, fs, startPose, inputs, errorState, 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) + + // err = CheckPlan(ackermanFrame, plan, 1, worldState, fs, startPose, inputs, errorState, 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 = CheckPlan(ackermanFrame, plan, 1, worldState, fs, startPose, inputs, errorState, 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) + + // err = CheckPlan(ackermanFrame, plan, 2, worldState, fs, startPose, inputs, errorState, 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()) + // errorState := spatialmath.PoseDelta(pathPose, startPose) + + // err = CheckPlan(ackermanFrame, plan, 2, worldState, fs, startPose, inputs, errorState, math.Inf(1), logger) + // test.That(t, err, test.ShouldBeNil) + // }) } func planToTpspaceRec(plan Plan, f referenceframe.Frame) ([]node, error) { diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 7eba35d3b6e..bafdddb8104 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -313,11 +313,7 @@ func (mr *moveRequest) getCurrentInputsAndPosition(ctx context.Context) (map[str return nil, nil, err } currentInputs[mr.kinematicBase.Name().ShortName()+"ExecutionFrame"] = referenceframe.FloatsToInputs( - []float64{ - pif.Pose().Point().X, - pif.Pose().Point().Y, - pif.Pose().Orientation().OrientationVectorRadians().Theta, - }, + []float64{pif.Pose().Point().X, pif.Pose().Point().Y, pif.Pose().Orientation().OrientationVectorRadians().Theta}, ) return currentInputs, pif, nil } From 5e39ce86de8bdfe9773b5656f0648bbc3373fc09 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 1 May 2024 12:34:13 -0400 Subject: [PATCH 017/126] fix segment logic --- motionplan/motionPlanner.go | 17 ++--------------- 1 file changed, 2 insertions(+), 15 deletions(-) diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index c8a9aa32577..2818ca6db46 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -540,7 +540,6 @@ func CheckPlan( // iterate through remaining plan and append remaining segments to check for i := wayPointIdx; i < len(offsetPlan.Path())-1; i++ { - // do we want to add a relative special case here or in the anon function currInput := offsetPlan.Trajectory()[i] nextInput := offsetPlan.Trajectory()[i+1] if relative { @@ -572,27 +571,15 @@ func CheckPlan( 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 } - // Add new comment here + // define State which only houses inputs, pose information not needed interpolatedState := &ik.State{Frame: sf} - if relative { - // I AM PRETTY SURE THAT ORDER MATTERS HERE IN TERMS OF STUFF? - interpolatedState.Configuration = []frame.Input{ - {Value: segment.StartPosition.Point().X}, - {Value: segment.StartPosition.Point().Y}, - {Value: segment.StartPosition.Orientation().OrientationVectorDegrees().Theta}, - interpConfig[0], interpConfig[1], interpConfig[2], - } - } else { - interpolatedState.Configuration = interpConfig - } - fmt.Println("interpolatedState: ", interpolatedState) + interpolatedState.Configuration = interpConfig // 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 { From db3ca28acdbe9f0bbd77e8481ee841743a7a6808 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 1 May 2024 12:40:33 -0400 Subject: [PATCH 018/126] correct plannerSetUp inputs --- motionplan/motionPlanner.go | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 2818ca6db46..8ae0132b0a5 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -477,12 +477,13 @@ func CheckPlan( if err != nil { return err } - + plannerSetUpInputs := currentInputs + plannerSetUpInputs[checkFrame.Name()] = make([]frame.Input, len(checkFrame.DoF())) // setup the planOpts if sfPlanner.planOpts, err = sfPlanner.plannerSetupFromMoveRequest( currentPose, poses[len(poses)-1], - currentInputs, + plannerSetUpInputs, worldState, nil, // no pb.Constraints nil, // no plannOpts From d8ab4ce300168bd1db90ecdbf4ca7034b04b8ea3 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 1 May 2024 14:32:55 -0400 Subject: [PATCH 019/126] lint + revert tpspace test - not finished yet --- motionplan/motionPlanner.go | 5 +- motionplan/tpSpaceRRT_test.go | 267 ++++++++++++++-------------------- 2 files changed, 111 insertions(+), 161 deletions(-) diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 8ae0132b0a5..036c5a26b66 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -17,7 +17,6 @@ import ( "go.viam.com/rdk/logging" "go.viam.com/rdk/motionplan/ik" - "go.viam.com/rdk/referenceframe" frame "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -544,10 +543,10 @@ func CheckPlan( currInput := offsetPlan.Trajectory()[i] nextInput := offsetPlan.Trajectory()[i+1] if relative { - currInput[checkFrame.Name()+"ExecutionFrame"] = referenceframe.FloatsToInputs( + currInput[checkFrame.Name()+"ExecutionFrame"] = frame.FloatsToInputs( []float64{poses[i].Point().X, poses[i].Point().Y, poses[i].Orientation().OrientationVectorRadians().Theta}, ) - nextInput[checkFrame.Name()+"ExecutionFrame"] = referenceframe.FloatsToInputs( + nextInput[checkFrame.Name()+"ExecutionFrame"] = frame.FloatsToInputs( []float64{poses[i+1].Point().X, poses[i+1].Point().Y, poses[i+1].Orientation().OrientationVectorRadians().Theta}, ) } diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index acc1053388d..91c509a20f7 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -4,7 +4,6 @@ package motionplan import ( "context" - "fmt" "math" "math/rand" "testing" @@ -282,171 +281,123 @@ func TestPtgCheckPlan(t *testing.T) { test.That(t, err, test.ShouldBeNil) plan, err := newRRTPlan(nodes, sf, true) test.That(t, err, test.ShouldBeNil) - fmt.Println(plan.Path()) - fmt.Println(plan.Trajectory()) - steps, err := plan.Trajectory().GetFrameInputs("ackframe") - test.That(t, err, test.ShouldBeNil) - firstNonZeroInput := steps[1] - myCustomInput := []referenceframe.Input{ - firstNonZeroInput[0], firstNonZeroInput[1], {Value: 2000}, {Value: 2000}, - } - myCustomEplisonInput := []referenceframe.Input{ - firstNonZeroInput[0], firstNonZeroInput[1], {Value: 2000}, {Value: 2005}, - } + startPose := spatialmath.NewPoseFromPoint(r3.Vector{0, 0, 0}) + errorState := startPose + inputs := plan.Trajectory()[0] - pose1, err := ackermanFrame.Transform(myCustomInput) - test.That(t, err, test.ShouldBeNil) - pose2, err := ackermanFrame.Transform(myCustomEplisonInput) - test.That(t, err, test.ShouldBeNil) + t.Run("base case - validate plan without obstacles", func(t *testing.T) { + err := CheckPlan(ackermanFrame, plan, 0, nil, fs, startPose, inputs, errorState, math.Inf(1), logger) + test.That(t, err, test.ShouldBeNil) + }) - fmt.Println("pose1: ", spatialmath.PoseToProtobuf(pose1)) - fmt.Println("pose2: ", spatialmath.PoseToProtobuf(pose2)) + 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) - myOtherCustomInput := []referenceframe.Input{ - firstNonZeroInput[0], firstNonZeroInput[1], {Value: 2000}, firstNonZeroInput[3], - } - pose3, err := ackermanFrame.Transform(myCustomEplisonInput) - test.That(t, err, test.ShouldBeNil) + geoms := []spatialmath.Geometry{obstacle} + gifs := []*referenceframe.GeometriesInFrame{referenceframe.NewGeometriesInFrame(referenceframe.World, geoms)} - var pif *referenceframe.PoseInFrame - for _, v := range plan.Path()[1] { - pif = v - } - fmt.Println("pif: ", spatialmath.PoseToProtobuf(pif.Pose())) - - customSegment := &ik.Segment{ - Frame: ackermanFrame, - StartPosition: pose3, - StartConfiguration: myOtherCustomInput, - EndConfiguration: myOtherCustomInput, - EndPosition: pif.Pose(), - } + worldState, err := referenceframe.NewWorldState(gifs, nil) + test.That(t, err, test.ShouldBeNil) - interpolations, err := interpolateSegment(customSegment, 2) + err = CheckPlan(ackermanFrame, plan, 0, worldState, fs, startPose, inputs, errorState, 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) - for _, i := range interpolations { - fmt.Println("i: ", i) - } - // ackermanFrame.Transform() - - // startPose := spatialmath.NewPoseFromPoint(r3.Vector{0, 0, 0}) - // errorState := startPose - // inputs := plan.Trajectory()[0] - - // t.Run("base case - validate plan without obstacles", func(t *testing.T) { - // err := CheckPlan(ackermanFrame, plan, 0, nil, fs, startPose, inputs, errorState, 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 = CheckPlan(ackermanFrame, plan, 0, worldState, fs, startPose, inputs, errorState, 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) - - // err = CheckPlan(ackermanFrame, plan, 1, worldState, fs, startPose, inputs, errorState, 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 = CheckPlan(ackermanFrame, plan, 1, worldState, fs, startPose, inputs, errorState, 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) - - // err = CheckPlan(ackermanFrame, plan, 2, worldState, fs, startPose, inputs, errorState, 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()) - // errorState := spatialmath.PoseDelta(pathPose, startPose) - - // err = CheckPlan(ackermanFrame, plan, 2, worldState, fs, startPose, inputs, errorState, math.Inf(1), logger) - // 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) + + err = CheckPlan(ackermanFrame, plan, 1, worldState, fs, startPose, inputs, errorState, 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 = CheckPlan(ackermanFrame, plan, 1, worldState, fs, startPose, inputs, errorState, 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) + + err = CheckPlan(ackermanFrame, plan, 2, worldState, fs, startPose, inputs, errorState, 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()) + errorState := spatialmath.PoseDelta(pathPose, startPose) + + err = CheckPlan(ackermanFrame, plan, 2, worldState, fs, startPose, inputs, errorState, math.Inf(1), logger) + test.That(t, err, test.ShouldBeNil) + }) } func planToTpspaceRec(plan Plan, f referenceframe.Frame) ([]node, error) { From 6917eb9577ade171c0b7385182409821da5bd97c Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 1 May 2024 15:04:49 -0400 Subject: [PATCH 020/126] revert test file while updating TestGetTransientDetections --- services/motion/builtin/builtin_test.go | 112 +++++++++++------------- 1 file changed, 50 insertions(+), 62 deletions(-) diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index e0eba93d41a..b23c749be70 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -512,8 +512,8 @@ func TestObstacleReplanningGlobe(t *testing.T) { } func TestObstacleReplanningSlam(t *testing.T) { - // cameraToBase := spatialmath.NewPose(r3.Vector{0, 0, 0}, &spatialmath.OrientationVectorDegrees{OY: 1, Theta: -90}) - // cameraToBaseInv := spatialmath.PoseInverse(cameraToBase) + cameraToBase := spatialmath.NewPose(r3.Vector{0, 0, 0}, &spatialmath.OrientationVectorDegrees{OY: 1, Theta: -90}) + cameraToBaseInv := spatialmath.PoseInverse(cameraToBase) ctx := context.Background() origin := spatialmath.NewPose( @@ -521,68 +521,62 @@ func TestObstacleReplanningSlam(t *testing.T) { &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: -90}, ) - // boxWrld, err := spatialmath.NewBox( - // spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 0, Z: 0}), - // r3.Vector{X: 50, Y: 50, Z: 50}, "box-obstacle", - // ) - // test.That(t, err, test.ShouldBeNil) + boxWrld, err := spatialmath.NewBox( + spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 0, Z: 0}), + r3.Vector{X: 50, Y: 50, Z: 50}, "box-obstacle", + ) + test.That(t, err, test.ShouldBeNil) - _, ms := createMoveOnMapEnvironment( + kb, ms := createMoveOnMapEnvironment( ctx, t, "pointcloud/cardboardOcto.pcd", 50, origin, ) defer ms.Close(ctx) - cI, _, err := ms.(*builtIn).fsService.CurrentInputs(ctx) + visSrvc, ok := ms.(*builtIn).visionServices[vision.Named("test-vision")].(*inject.VisionService) + test.That(t, ok, test.ShouldBeTrue) + i := 0 + visSrvc.GetObjectPointCloudsFunc = func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { + if i == 0 { + i++ + return []*viz.Object{}, nil + } + currentPif, err := kb.CurrentPosition(ctx) + test.That(t, err, test.ShouldBeNil) + + relativeBox := boxWrld.Transform(spatialmath.PoseInverse(currentPif.Pose())).Transform(cameraToBaseInv) + detection, err := viz.NewObjectWithLabel(pointcloud.New(), "test-case-1-detection", relativeBox.ToProtobuf()) + test.That(t, err, test.ShouldBeNil) + + return []*viz.Object{detection}, nil + } + + obstacleDetectorSlice := []motion.ObstacleDetectorName{ + {VisionServiceName: vision.Named("test-vision"), CameraName: camera.Named("test-camera")}, + } + req := motion.MoveOnMapReq{ + ComponentName: base.Named("test-base"), + Destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 800, Y: 0, Z: 0}), + SlamName: slam.Named("test_slam"), + MotionCfg: &motion.MotionConfiguration{ + PositionPollingFreqHz: 1, ObstaclePollingFreqHz: 100, PlanDeviationMM: 1, ObstacleDetectors: obstacleDetectorSlice, + }, + // TODO: add back "max_replans": 1 to extra + Extra: map[string]interface{}{"smooth_iter": 0}, + } + + executionID, err := ms.MoveOnMap(ctx, req) + test.That(t, err, test.ShouldBeNil) + + timeoutCtx, timeoutFn := context.WithTimeout(ctx, time.Second*15) + defer timeoutFn() + err = motion.PollHistoryUntilSuccessOrError(timeoutCtx, ms, time.Millisecond, motion.PlanHistoryReq{ + ComponentName: req.ComponentName, + ExecutionID: executionID, + LastPlanOnly: true, + }) test.That(t, err, test.ShouldBeNil) - fmt.Println("cI: ", cI) - - // visSrvc, ok := ms.(*builtIn).visionServices[vision.Named("test-vision")].(*inject.VisionService) - // test.That(t, ok, test.ShouldBeTrue) - // i := 0 - // visSrvc.GetObjectPointCloudsFunc = func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { - // if i == 0 { - // i++ - // return []*viz.Object{}, nil - // } - // currentPif, err := kb.CurrentPosition(ctx) - // test.That(t, err, test.ShouldBeNil) - - // relativeBox := boxWrld.Transform(spatialmath.PoseInverse(currentPif.Pose())).Transform(cameraToBaseInv) - // detection, err := viz.NewObjectWithLabel(pointcloud.New(), "test-case-1-detection", relativeBox.ToProtobuf()) - // test.That(t, err, test.ShouldBeNil) - - // return []*viz.Object{detection}, nil - // } - - // obstacleDetectorSlice := []motion.ObstacleDetectorName{ - // {VisionServiceName: vision.Named("test-vision"), CameraName: camera.Named("test-camera")}, - // } - // req := motion.MoveOnMapReq{ - // ComponentName: base.Named("test-base"), - // Destination: spatialmath.NewPoseFromPoint(r3.Vector{X: 800, Y: 0, Z: 0}), - // SlamName: slam.Named("test_slam"), - // MotionCfg: &motion.MotionConfiguration{ - // PositionPollingFreqHz: 1, ObstaclePollingFreqHz: 100, PlanDeviationMM: 1, ObstacleDetectors: obstacleDetectorSlice, - // }, - // // TODO: add back "max_replans": 1 to extra - // Extra: map[string]interface{}{"smooth_iter": 0}, - // } - - // executionID, err := ms.MoveOnMap(ctx, req) - // test.That(t, err, test.ShouldBeNil) - - // timeoutCtx, timeoutFn := context.WithTimeout(ctx, time.Second*15) - // defer timeoutFn() - // - // err = motion.PollHistoryUntilSuccessOrError(timeoutCtx, ms, time.Millisecond, motion.PlanHistoryReq{ - // ComponentName: req.ComponentName, - // ExecutionID: executionID, - // LastPlanOnly: true, - // }) - // - // test.That(t, err, test.ShouldBeNil) } func TestMultiplePieces(t *testing.T) { @@ -2619,33 +2613,27 @@ 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}), }, { 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: "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: "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: "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}), }, } From 7e25a8d72f79e713f319849630aa0c40f710a67e Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 2 May 2024 15:46:38 -0400 Subject: [PATCH 021/126] motion builtin-tests are passinggit statusgit status lets gooooooooo --- .../base/kinematicbase/fake_kinematics.go | 4 ++- .../base/kinematicbase/ptgKinematics.go | 6 ++-- motionplan/constraint.go | 20 +++++++++++++ motionplan/motionPlanner.go | 10 +++++++ motionplan/planManager.go | 21 +++++++++++++ motionplan/solverFrame.go | 29 ++++++++++++++++++ referenceframe/frame_system.go | 7 +++++ services/motion/builtin/builtin_test.go | 30 +++++++------------ services/motion/builtin/move_request.go | 19 ++++++++++++ 9 files changed, 123 insertions(+), 23 deletions(-) diff --git a/components/base/kinematicbase/fake_kinematics.go b/components/base/kinematicbase/fake_kinematics.go index 680fad53f91..76ab00031e2 100644 --- a/components/base/kinematicbase/fake_kinematics.go +++ b/components/base/kinematicbase/fake_kinematics.go @@ -5,6 +5,7 @@ package kinematicbase import ( "context" "errors" + "fmt" "sync" "time" @@ -191,11 +192,12 @@ func WrapWithFakePTGKinematics( if err != nil { return nil, err } + fmt.Println("planningFrame.DoF(): ", planningFrame.DoF()) // construct execution frame executionFrame, err := referenceframe.New2DMobileModelFrame( b.Name().ShortName()+"ExecutionFrame", - planningFrame.DoF(), + planningFrame.DoF()[:3], geometries[0], ) if err != nil { diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index 2ddd98e8b5c..eb20899f3ab 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -7,6 +7,7 @@ package kinematicbase import ( "context" "errors" + "fmt" "sync" "go.viam.com/rdk/components/base" @@ -56,6 +57,7 @@ func wrapWithPTGKinematics( localizer motion.Localizer, options Options, ) (KinematicBase, error) { + fmt.Println("hello there") properties, err := b.Properties(ctx, nil) if err != nil { return nil, err @@ -132,11 +134,11 @@ func wrapWithPTGKinematics( } origin = originPIF.Pose() } - + fmt.Println("b.Name().ShortName()+ExecutionFrame: ", b.Name().ShortName()+"ExecutionFrame") // construct executionFrame executionFrame, err := referenceframe.New2DMobileModelFrame( b.Name().ShortName()+"ExecutionFrame", - planningFrame.DoF(), + planningFrame.DoF()[:3], geometries[0], ) if err != nil { diff --git a/motionplan/constraint.go b/motionplan/constraint.go index 16a363ccb51..e541dcd8cf1 100644 --- a/motionplan/constraint.go +++ b/motionplan/constraint.go @@ -247,12 +247,29 @@ func createAllCollisionConstraints( for _, geom := range worldGeometries { if octree, ok := geom.(*pointcloud.BasicOctree); ok { if zeroCG == nil { + fmt.Println("WE ARE NOW CONSTRUCTING ZEROCG SINCE WHAT WE GOT WAS NIL") + fmt.Println("NOW PRINTING movingRobotGeometries") + for _, g := range movingRobotGeometries { + fmt.Println("g.Pose(): ", spatial.PoseToProtobuf(g.Pose())) + } + fmt.Println("NOW PRINTING worldGeometries") + for _, g := range worldGeometries { + fmt.Println("g.Pose(): ", spatial.PoseToProtobuf(g.Pose())) + } + fmt.Println("NOW PRINTING allowedCollisions") + for _, c := range allowedCollisions { + fmt.Println("c: ", c) + } + fmt.Println("DONE") zeroCG, err = setupZeroCG(movingRobotGeometries, worldGeometries, allowedCollisions, collisionBufferMM) if err != nil { return nil, err } + } + fmt.Println("PRINTING EXISTING COLLISIONS") for _, collision := range zeroCG.collisions(collisionBufferMM) { + fmt.Println("collision: ", collision) if collision.name1 == octree.Label() { return nil, fmt.Errorf("starting collision between SLAM map and %s, cannot move", collision.name2) } else if collision.name2 == octree.Label() { @@ -335,6 +352,9 @@ func NewCollisionConstraint( return false } internalGeoms = internal.Geometries() + // for _, g := range internalGeoms { + // fmt.Println("g.Pose: ", spatial.PoseToProtobuf(g.Pose())) + // } case state.Position != nil: // If we didn't pass a Configuration, but we do have a Position, then get the geometries at the zero state and // transform them to the Position diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 036c5a26b66..50d1e0a9229 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -448,6 +448,8 @@ func CheckPlan( if len(plan.Path()) < 1 { return errors.New("plan must have at least one element") } + fmt.Println("WE ARE MAKING THE SOLVERFRAME NOW") + fmt.Println("fs.FrameNames(): ", fs.FrameNames()) // construct solverFrame // Note that this requires all frames which move as part of the plan, to have an @@ -495,10 +497,12 @@ func CheckPlan( if relative { // get checkFrame's currentInputs // *currently* it is guaranteed that a relative frame will constitute 100% of a solver frame's dof + fmt.Println("currentInputs: ", currentInputs) checkFrameCurrentInputs, err := sf.mapToSlice(currentInputs) if err != nil { return err } + fmt.Println("checkFrameCurrentInputs: ", checkFrameCurrentInputs) // pre-pend to segments so we can connect to the input we have not finished actuating yet segments = append(segments, &ik.Segment{ @@ -557,6 +561,10 @@ func CheckPlan( segments = append(segments, segment) } + for _, s := range segments { + fmt.Println("s: ", s) + } + // 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. @@ -580,6 +588,8 @@ func CheckPlan( // define State which only houses inputs, pose information not needed interpolatedState := &ik.State{Frame: sf} interpolatedState.Configuration = interpConfig + fmt.Println("interpConfig: ", interpConfig) + fmt.Println("interpolatedState: ", interpolatedState) // 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 { diff --git a/motionplan/planManager.go b/motionplan/planManager.go index da6d19150fe..f96707b709c 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -56,6 +56,7 @@ func newPlanManager( // Any constraints, etc, will be held for the entire motion. func (pm *planManager) PlanSingleWaypoint(ctx context.Context, request *PlanRequest, seedPlan Plan) (Plan, error) { if pm.useTPspace { + fmt.Println("PLANNING IN RELATIVE TERMS") return pm.planRelativeWaypoint(ctx, request, seedPlan) } @@ -788,12 +789,32 @@ func (pm *planManager) planRelativeWaypoint(ctx context.Context, request *PlanRe return nil, err } goalPos := tf.(*referenceframe.PoseInFrame).Pose() + // maybe here we want to edit the FS...? + // this seems dangerous... + // pm.frame.fss = + copyOfFS := pm.frame.fss + transformFrame, err := referenceframe.NewStaticFrame("static-transform-bro", request.StartPose) + if err != nil { + return nil, err + } + daFS := referenceframe.NewEmptyFrameSystem("lolol") + err = daFS.AddFrame(transformFrame, daFS.World()) + if err != nil { + return nil, err + } + err = daFS.MergeFrameSystem(pm.frame.fss, transformFrame) + if err != nil { + return nil, err + } + pm.frame.fss = daFS + opt, err := pm.plannerSetupFromMoveRequest( startPose, goalPos, request.StartConfiguration, request.WorldState, request.ConstraintSpecs, request.Options, ) if err != nil { return nil, err } + pm.frame.fss = copyOfFS pm.planOpts = opt opt.SetGoal(goalPos) diff --git a/motionplan/solverFrame.go b/motionplan/solverFrame.go index 8dfd3c89e28..be173622eea 100644 --- a/motionplan/solverFrame.go +++ b/motionplan/solverFrame.go @@ -2,6 +2,7 @@ package motionplan import ( "errors" + "fmt" "strings" "go.uber.org/multierr" @@ -48,6 +49,7 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, if solveFrame == nil { return nil, frame.NewFrameMissingError(solveFrameName) } + // fmt.Println("SOLVER FRAME fs.FrameNames(): ", fs.FrameNames()) solveFrameList, err := fs.TracebackFrame(solveFrame) if err != nil { return nil, err @@ -79,7 +81,17 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, if err != nil { return nil, err } + // fmt.Println("pivotFrame.Name(): ", pivotFrame.Name()) if pivotFrame.Name() == frame.World { + // fmt.Println("printing members of solveFrameList") + // for _, f := range solveFrameList { + // fmt.Println("f.Name(): ", f.Name()) + // } + // fmt.Println("printing members of goalFrameList") + // for _, f := range goalFrameList { + // fmt.Println("f.Name(): ", f.Name()) + // } + // fmt.Println("DONE") frames = uniqInPlaceSlice(append(solveFrameList, goalFrameList...)) moving, err = movingFS(solveFrameList) if err != nil { @@ -103,6 +115,7 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, break } dof += len(frame.DoF()) + // fmt.Println("will now append: ", frame.Name()) frames = append(frames, frame) solveMovingList = append(solveMovingList, frame) } @@ -111,6 +124,7 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, break } dof += len(frame.DoF()) + // fmt.Println("will now append: ", frame.Name()) frames = append(frames, frame) goalMovingList = append(goalMovingList, frame) } @@ -119,11 +133,13 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, if dof == 0 { worldRooted = true frames = solveFrameList + // fmt.Println("we are here 1") moving, err = movingFS(solveFrameList) if err != nil { return nil, err } } else { + // fmt.Println("we are here 2") // Get all child nodes of pivot node moving, err = movingFS(solveMovingList) if err != nil { @@ -148,6 +164,8 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, delete(origSeed, frame.Name()) } + // fmt.Println("THIS IS THE ORIG SEED: ", origSeed) + var ptgs []tpspace.PTGSolver anyPTG := false // Whether PTG frames have been observed // TODO(nf): uncomment @@ -220,7 +238,9 @@ func (sf *solverFrame) Interpolate(from, to []frame.Input, by float64) ([]frame. posIdx = dof var interpSub []frame.Input var err error + // fmt.Println("currFrame.Name(): ", currFrame.Name()) if strings.Contains(currFrame.Name(), "ExecutionFrame") { + fmt.Println("this conditional is hit!") interp = append(interp, from...) continue } @@ -320,8 +340,17 @@ func (sf *solverFrame) movingFrame(name string) bool { // the inputs together in the order of the frames in sf.frames. func (sf *solverFrame) mapToSlice(inputMap map[string][]frame.Input) ([]frame.Input, error) { var inputs []frame.Input + fmt.Println("inputMap: ", inputMap) + fmt.Println("printing all members of sf.frames below") + for _, f := range sf.frames { + fmt.Println("f.Name(): ", f.Name()) + } + fmt.Println("DONE -- printing all members of sf.frames") for _, f := range sf.frames { if len(f.DoF()) == 0 { + fmt.Println("we are continuing here?") + fmt.Println("f.Name(): ", f.Name()) + fmt.Println("f.DoF(): ", f.DoF()) continue } input, err := frame.GetFrameInputs(f, inputMap) diff --git a/referenceframe/frame_system.go b/referenceframe/frame_system.go index 5a06a5f0fe4..cf25296fb3a 100644 --- a/referenceframe/frame_system.go +++ b/referenceframe/frame_system.go @@ -68,6 +68,13 @@ type FrameSystem interface { // frame's children and parentage to replacementFrame. The original frame is removed entirely from the frame system. // replacementFrame is not allowed to exist within the frame system at the time of the call. ReplaceFrame(replacementFrame Frame) error + + // consider adding method `AddFrameBetween` to this interface for ease of adding a static transform between the + // kb planning frame and the world when it comes to planning + // also when it comed to adding a execution frame between the execution frame + // new: should not be a method on the interface but should instead just + // exist as a helper function which returns an augmented copy of the + // pass in framesystem } // FrameSystemPart is used to collect all the info need from a named robot part to build the frame node in a frame system. diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index b23c749be70..7533c8a9ac6 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -1328,7 +1328,7 @@ func TestMoveOnMapStaticObs(t *testing.T) { test.That(t, err, test.ShouldBeNil) test.That(t, len(plan.Path()), test.ShouldBeGreaterThan, 2) - // place obstacle in opposte position and show that the generate path + // place obstacle in opposite position and show that the generate path // collides with obstacleLeft wrldSt, err := referenceframe.NewWorldState( @@ -1340,18 +1340,25 @@ func TestMoveOnMapStaticObs(t *testing.T) { }, nil, ) test.That(t, err, test.ShouldBeNil) + fmt.Println("PRINTING mr.absoluteFS.FrameNames()") + for _, name := range mr.absoluteFS.FrameNames() { + fmt.Println("name: ", name) + } + currentInputs := referenceframe.StartPositions(mr.absoluteFS) + currentInputs["test-baseExecutionFrame"] = referenceframe.FloatsToInputs([]float64{0.58772e3, -0.80826e3, 0}) + fmt.Println("currentInputs :", currentInputs) err = motionplan.CheckPlan( mr.planRequest.Frame, plan, 1, wrldSt, - mr.planRequest.FrameSystem, + mr.absoluteFS, spatialmath.NewPose( r3.Vector{X: 0.58772e3, Y: -0.80826e3, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 0}, ), - referenceframe.StartPositions(mr.planRequest.FrameSystem), + currentInputs, spatialmath.NewZeroPose(), lookAheadDistanceMM, logger, @@ -2566,7 +2573,6 @@ func TestMoveCallInputs(t *testing.T) { } func TestGetTransientDetections(t *testing.T) { - t.Parallel() ctx := context.Background() _, ms := createMoveOnMapEnvironment( @@ -2620,22 +2626,6 @@ func TestGetTransientDetections(t *testing.T) { name: "relative - SLAM/base theta does not matter", detectionPose: spatialmath.NewPose(r3.Vector{4, 10, -8}, &spatialmath.OrientationVectorDegrees{OY: 1, Theta: -90}), }, - { - name: "absolute - SLAM theta: 0, base theta: -90 == 270", - detectionPose: spatialmath.NewPose(r3.Vector{6, -14, -8}, &spatialmath.OrientationVectorDegrees{OX: 1, Theta: -90}), - }, - { - name: "absolute - SLAM theta: 90, base theta: 0", - detectionPose: spatialmath.NewPose(r3.Vector{0, 0, -8}, &spatialmath.OrientationVectorDegrees{OY: 1, Theta: -90}), - }, - { - name: "absolute - SLAM theta: 180, base theta: 90", - detectionPose: spatialmath.NewPose(r3.Vector{-14, -6, -8}, &spatialmath.OrientationVectorDegrees{OX: -1, Theta: -90}), - }, - { - name: "absolute - SLAM theta: 270, base theta: 180", - detectionPose: spatialmath.NewPose(r3.Vector{-8, -20, -8}, &spatialmath.OrientationVectorDegrees{OY: -1, Theta: -90}), - }, } testFn := func(t *testing.T, tc testCase) { diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index bafdddb8104..c9ec617cbdb 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -113,6 +113,25 @@ func (mr *moveRequest) Plan(ctx context.Context) (motionplan.Plan, error) { return nil, err } + // currentPIF, err := mr.kinematicBase.CurrentPosition(ctx) + // if err != nil { + // return nil, err + // } + // transformFrame, err := referenceframe.NewStaticFrame("static-transform-bro", currentPIF.Pose()) + // if err != nil { + // return nil, err + // } + // daFS := referenceframe.NewEmptyFrameSystem("lolol") + // err = daFS.AddFrame(transformFrame, daFS.World()) + // if err != nil { + // return nil, err + // } + // err = daFS.MergeFrameSystem(planRequestCopy.FrameSystem, transformFrame) + // if err != nil { + // return nil, err + // } + // planRequestCopy.FrameSystem = daFS + // TODO(RSDK-5634): this should pass in mr.seedplan and the appropriate replanCostFactor once this bug is found and fixed. return motionplan.Replan(ctx, &planRequestCopy, nil, 0) } From ce18c039165e64a436434668f15eb8fcb4a4497a Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 2 May 2024 15:50:54 -0400 Subject: [PATCH 022/126] remove un-needed code commented out code --- services/motion/builtin/move_request.go | 19 ------------------- 1 file changed, 19 deletions(-) diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index c9ec617cbdb..bafdddb8104 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -113,25 +113,6 @@ func (mr *moveRequest) Plan(ctx context.Context) (motionplan.Plan, error) { return nil, err } - // currentPIF, err := mr.kinematicBase.CurrentPosition(ctx) - // if err != nil { - // return nil, err - // } - // transformFrame, err := referenceframe.NewStaticFrame("static-transform-bro", currentPIF.Pose()) - // if err != nil { - // return nil, err - // } - // daFS := referenceframe.NewEmptyFrameSystem("lolol") - // err = daFS.AddFrame(transformFrame, daFS.World()) - // if err != nil { - // return nil, err - // } - // err = daFS.MergeFrameSystem(planRequestCopy.FrameSystem, transformFrame) - // if err != nil { - // return nil, err - // } - // planRequestCopy.FrameSystem = daFS - // TODO(RSDK-5634): this should pass in mr.seedplan and the appropriate replanCostFactor once this bug is found and fixed. return motionplan.Replan(ctx, &planRequestCopy, nil, 0) } From dfeec43bb40085dfa0380bdbf11a5b3b61c78127 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 3 May 2024 00:35:31 -0400 Subject: [PATCH 023/126] clean some print statements --- .../base/kinematicbase/fake_kinematics.go | 2 - .../base/kinematicbase/ptgKinematics.go | 4 +- motionplan/constraint.go | 25 +--- motionplan/motionPlanner.go | 20 +-- motionplan/planManager.go | 26 ++-- motionplan/solverFrame.go | 41 ++---- motionplan/tpSpaceRRT_test.go | 118 +++++++++++------- services/motion/builtin/builtin_test.go | 6 +- 8 files changed, 117 insertions(+), 125 deletions(-) diff --git a/components/base/kinematicbase/fake_kinematics.go b/components/base/kinematicbase/fake_kinematics.go index 76ab00031e2..ec87af29d7c 100644 --- a/components/base/kinematicbase/fake_kinematics.go +++ b/components/base/kinematicbase/fake_kinematics.go @@ -5,7 +5,6 @@ package kinematicbase import ( "context" "errors" - "fmt" "sync" "time" @@ -192,7 +191,6 @@ func WrapWithFakePTGKinematics( if err != nil { return nil, err } - fmt.Println("planningFrame.DoF(): ", planningFrame.DoF()) // construct execution frame executionFrame, err := referenceframe.New2DMobileModelFrame( diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index eb20899f3ab..f68f56f9d0c 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -7,7 +7,6 @@ package kinematicbase import ( "context" "errors" - "fmt" "sync" "go.viam.com/rdk/components/base" @@ -57,7 +56,6 @@ func wrapWithPTGKinematics( localizer motion.Localizer, options Options, ) (KinematicBase, error) { - fmt.Println("hello there") properties, err := b.Properties(ctx, nil) if err != nil { return nil, err @@ -134,7 +132,7 @@ func wrapWithPTGKinematics( } origin = originPIF.Pose() } - fmt.Println("b.Name().ShortName()+ExecutionFrame: ", b.Name().ShortName()+"ExecutionFrame") + // construct executionFrame executionFrame, err := referenceframe.New2DMobileModelFrame( b.Name().ShortName()+"ExecutionFrame", diff --git a/motionplan/constraint.go b/motionplan/constraint.go index e541dcd8cf1..91612049755 100644 --- a/motionplan/constraint.go +++ b/motionplan/constraint.go @@ -241,35 +241,20 @@ func createAllCollisionConstraints( var err error if len(worldGeometries) > 0 { + // Check if a moving geometry is in collision with a pointcloud. If so, error. // TODO: This is not the most robust way to deal with this but is better than driving through walls. var zeroCG *collisionGraph for _, geom := range worldGeometries { if octree, ok := geom.(*pointcloud.BasicOctree); ok { if zeroCG == nil { - fmt.Println("WE ARE NOW CONSTRUCTING ZEROCG SINCE WHAT WE GOT WAS NIL") - fmt.Println("NOW PRINTING movingRobotGeometries") - for _, g := range movingRobotGeometries { - fmt.Println("g.Pose(): ", spatial.PoseToProtobuf(g.Pose())) - } - fmt.Println("NOW PRINTING worldGeometries") - for _, g := range worldGeometries { - fmt.Println("g.Pose(): ", spatial.PoseToProtobuf(g.Pose())) - } - fmt.Println("NOW PRINTING allowedCollisions") - for _, c := range allowedCollisions { - fmt.Println("c: ", c) - } - fmt.Println("DONE") zeroCG, err = setupZeroCG(movingRobotGeometries, worldGeometries, allowedCollisions, collisionBufferMM) if err != nil { return nil, err } } - fmt.Println("PRINTING EXISTING COLLISIONS") for _, collision := range zeroCG.collisions(collisionBufferMM) { - fmt.Println("collision: ", collision) if collision.name1 == octree.Label() { return nil, fmt.Errorf("starting collision between SLAM map and %s, cannot move", collision.name2) } else if collision.name2 == octree.Label() { @@ -278,7 +263,6 @@ func createAllCollisionConstraints( } } } - // create constraint to keep moving geometries from hitting world state obstacles obstacleConstraint, err := NewCollisionConstraint(movingRobotGeometries, worldGeometries, allowedCollisions, false, collisionBufferMM) if err != nil { @@ -286,7 +270,6 @@ func createAllCollisionConstraints( } constraintMap[defaultObstacleConstraintDesc] = obstacleConstraint } - if len(staticRobotGeometries) > 0 { // create constraint to keep moving geometries from hitting other geometries on robot that are not moving robotConstraint, err := NewCollisionConstraint( @@ -308,6 +291,7 @@ func createAllCollisionConstraints( return nil, err } constraintMap[defaultSelfCollisionConstraintDesc] = selfCollisionConstraint + fmt.Println("constraintMap: ", constraintMap) } return constraintMap, nil } @@ -352,12 +336,7 @@ func NewCollisionConstraint( return false } internalGeoms = internal.Geometries() - // for _, g := range internalGeoms { - // fmt.Println("g.Pose: ", spatial.PoseToProtobuf(g.Pose())) - // } case state.Position != nil: - // If we didn't pass a Configuration, but we do have a Position, then get the geometries at the zero state and - // transform them to the Position internal, err := state.Frame.Geometries(make([]referenceframe.Input, len(state.Frame.DoF()))) if err != nil { return false diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 50d1e0a9229..777d2f1326c 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -448,8 +448,13 @@ func CheckPlan( if len(plan.Path()) < 1 { return errors.New("plan must have at least one element") } - fmt.Println("WE ARE MAKING THE SOLVERFRAME NOW") - fmt.Println("fs.FrameNames(): ", fs.FrameNames()) + + logger.Debugf("CheckPlan inputs: \n currentPosition: %v\n currentInputs: %v\n errorState: %v\n worldstate: %s", + spatialmath.PoseToProtobuf(currentPose), + currentInputs, + spatialmath.PoseToProtobuf(errorState), + worldState.String(), + ) // construct solverFrame // Note that this requires all frames which move as part of the plan, to have an @@ -497,12 +502,10 @@ func CheckPlan( if relative { // get checkFrame's currentInputs // *currently* it is guaranteed that a relative frame will constitute 100% of a solver frame's dof - fmt.Println("currentInputs: ", currentInputs) checkFrameCurrentInputs, err := sf.mapToSlice(currentInputs) if err != nil { return err } - fmt.Println("checkFrameCurrentInputs: ", checkFrameCurrentInputs) // pre-pend to segments so we can connect to the input we have not finished actuating yet segments = append(segments, &ik.Segment{ @@ -562,7 +565,7 @@ func CheckPlan( } for _, s := range segments { - fmt.Println("s: ", s) + fmt.Println("segment: ", s) } // go through segments and check that we satisfy constraints @@ -588,8 +591,11 @@ func CheckPlan( // define State which only houses inputs, pose information not needed interpolatedState := &ik.State{Frame: sf} interpolatedState.Configuration = interpConfig - fmt.Println("interpConfig: ", interpConfig) - fmt.Println("interpolatedState: ", interpolatedState) + sfTfPose, err := sf.Transform(interpConfig) + if err != nil { + return err + } + fmt.Println("sfTfPose: ", spatialmath.PoseToProtobuf(sfTfPose)) // 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 { diff --git a/motionplan/planManager.go b/motionplan/planManager.go index f96707b709c..180c259d3a5 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -56,7 +56,6 @@ func newPlanManager( // Any constraints, etc, will be held for the entire motion. func (pm *planManager) PlanSingleWaypoint(ctx context.Context, request *PlanRequest, seedPlan Plan) (Plan, error) { if pm.useTPspace { - fmt.Println("PLANNING IN RELATIVE TERMS") return pm.planRelativeWaypoint(ctx, request, seedPlan) } @@ -542,6 +541,8 @@ func (pm *planManager) plannerSetupFromMoveRequest( if err != nil { return nil, err } + fmt.Println("collisionConstraints: ", collisionConstraints) + fmt.Println("LEN collisionConstraints: ", len(collisionConstraints)) for name, constraint := range collisionConstraints { opt.AddStateConstraint(name, constraint) } @@ -789,24 +790,27 @@ func (pm *planManager) planRelativeWaypoint(ctx context.Context, request *PlanRe return nil, err } goalPos := tf.(*referenceframe.PoseInFrame).Pose() - // maybe here we want to edit the FS...? - // this seems dangerous... - // pm.frame.fss = - copyOfFS := pm.frame.fss - transformFrame, err := referenceframe.NewStaticFrame("static-transform-bro", request.StartPose) + + // We need to create a separate ephemeral framesystem to place the + // kinematic base frame which only understands relative inputs, i.e. + // given what is provided to us by request.StartConfiguration cannot place us + // anywhere else but the origin + copyOfOriginalFS := pm.frame.fss + + transformFrame, err := referenceframe.NewStaticFrame("", request.StartPose) if err != nil { return nil, err } - daFS := referenceframe.NewEmptyFrameSystem("lolol") - err = daFS.AddFrame(transformFrame, daFS.World()) + fsWithTransform := referenceframe.NewEmptyFrameSystem("store-starting point") + err = fsWithTransform.AddFrame(transformFrame, fsWithTransform.World()) if err != nil { return nil, err } - err = daFS.MergeFrameSystem(pm.frame.fss, transformFrame) + err = fsWithTransform.MergeFrameSystem(pm.frame.fss, transformFrame) if err != nil { return nil, err } - pm.frame.fss = daFS + pm.frame.fss = fsWithTransform opt, err := pm.plannerSetupFromMoveRequest( startPose, goalPos, request.StartConfiguration, request.WorldState, request.ConstraintSpecs, request.Options, @@ -814,7 +818,7 @@ func (pm *planManager) planRelativeWaypoint(ctx context.Context, request *PlanRe if err != nil { return nil, err } - pm.frame.fss = copyOfFS + pm.frame.fss = copyOfOriginalFS pm.planOpts = opt opt.SetGoal(goalPos) diff --git a/motionplan/solverFrame.go b/motionplan/solverFrame.go index be173622eea..cacd2d81a77 100644 --- a/motionplan/solverFrame.go +++ b/motionplan/solverFrame.go @@ -49,7 +49,7 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, if solveFrame == nil { return nil, frame.NewFrameMissingError(solveFrameName) } - // fmt.Println("SOLVER FRAME fs.FrameNames(): ", fs.FrameNames()) + solveFrameList, err := fs.TracebackFrame(solveFrame) if err != nil { return nil, err @@ -81,17 +81,7 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, if err != nil { return nil, err } - // fmt.Println("pivotFrame.Name(): ", pivotFrame.Name()) if pivotFrame.Name() == frame.World { - // fmt.Println("printing members of solveFrameList") - // for _, f := range solveFrameList { - // fmt.Println("f.Name(): ", f.Name()) - // } - // fmt.Println("printing members of goalFrameList") - // for _, f := range goalFrameList { - // fmt.Println("f.Name(): ", f.Name()) - // } - // fmt.Println("DONE") frames = uniqInPlaceSlice(append(solveFrameList, goalFrameList...)) moving, err = movingFS(solveFrameList) if err != nil { @@ -115,7 +105,6 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, break } dof += len(frame.DoF()) - // fmt.Println("will now append: ", frame.Name()) frames = append(frames, frame) solveMovingList = append(solveMovingList, frame) } @@ -124,7 +113,6 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, break } dof += len(frame.DoF()) - // fmt.Println("will now append: ", frame.Name()) frames = append(frames, frame) goalMovingList = append(goalMovingList, frame) } @@ -133,13 +121,11 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, if dof == 0 { worldRooted = true frames = solveFrameList - // fmt.Println("we are here 1") moving, err = movingFS(solveFrameList) if err != nil { return nil, err } } else { - // fmt.Println("we are here 2") // Get all child nodes of pivot node moving, err = movingFS(solveMovingList) if err != nil { @@ -164,8 +150,6 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, delete(origSeed, frame.Name()) } - // fmt.Println("THIS IS THE ORIG SEED: ", origSeed) - var ptgs []tpspace.PTGSolver anyPTG := false // Whether PTG frames have been observed // TODO(nf): uncomment @@ -205,6 +189,8 @@ func (sf *solverFrame) Name() string { // Transform returns the pose between the two frames of this solver for a given set of inputs. func (sf *solverFrame) Transform(inputs []frame.Input) (spatial.Pose, error) { + fmt.Println("sf.DoF(): ", sf.DoF()) + fmt.Println("inputs: ", inputs) if len(inputs) != len(sf.DoF()) { return nil, frame.NewIncorrectInputLengthError(len(inputs), len(sf.DoF())) } @@ -222,6 +208,7 @@ func (sf *solverFrame) Transform(inputs []frame.Input) (spatial.Pose, error) { // Interpolate interpolates the given amount between the two sets of inputs. func (sf *solverFrame) Interpolate(from, to []frame.Input, by float64) ([]frame.Input, error) { + fmt.Println("WITHIN SF INTERPOLATE") if len(from) != len(sf.DoF()) { return nil, frame.NewIncorrectInputLengthError(len(from), len(sf.DoF())) } @@ -238,10 +225,13 @@ func (sf *solverFrame) Interpolate(from, to []frame.Input, by float64) ([]frame. posIdx = dof var interpSub []frame.Input var err error - // fmt.Println("currFrame.Name(): ", currFrame.Name()) + + fmt.Println("currFrame.DoF()", currFrame.DoF()) + fmt.Println("currFrame.Name()", currFrame.Name()) if strings.Contains(currFrame.Name(), "ExecutionFrame") { - fmt.Println("this conditional is hit!") - interp = append(interp, from...) + interp = append(interp, fromSubset...) + fmt.Println("appending from...: ", fromSubset) + fmt.Println("interp: ", interp) continue } interpSub, err = currFrame.Interpolate(fromSubset, toSubset, by) @@ -250,6 +240,8 @@ func (sf *solverFrame) Interpolate(from, to []frame.Input, by float64) ([]frame. } interp = append(interp, interpSub...) + fmt.Println("appending from...: ", interpSub) + fmt.Println("interp: ", interpSub) } return interp, nil } @@ -340,17 +332,8 @@ func (sf *solverFrame) movingFrame(name string) bool { // the inputs together in the order of the frames in sf.frames. func (sf *solverFrame) mapToSlice(inputMap map[string][]frame.Input) ([]frame.Input, error) { var inputs []frame.Input - fmt.Println("inputMap: ", inputMap) - fmt.Println("printing all members of sf.frames below") - for _, f := range sf.frames { - fmt.Println("f.Name(): ", f.Name()) - } - fmt.Println("DONE -- printing all members of sf.frames") for _, f := range sf.frames { if len(f.DoF()) == 0 { - fmt.Println("we are continuing here?") - fmt.Println("f.Name(): ", f.Name()) - fmt.Println("f.DoF(): ", f.DoF()) continue } input, err := frame.GetFrameInputs(f, inputMap) diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index 91c509a20f7..3d4792fa6ed 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -4,6 +4,7 @@ package motionplan import ( "context" + "fmt" "math" "math/rand" "testing" @@ -285,9 +286,29 @@ func TestPtgCheckPlan(t *testing.T) { startPose := spatialmath.NewPoseFromPoint(r3.Vector{0, 0, 0}) errorState := startPose inputs := plan.Trajectory()[0] + fmt.Println("plan.Trajectory(): ", plan.Trajectory()) + fmt.Println("plan.Path(): ", plan.Path()) + + // NOTE: WE NEED TO ADD AN EXECUTION FRAME TO THE CHECKING FRAMESYSTEM SINCE WE ONLY WANT TO RELY ON USING INPUTS + tfFrameSystem := referenceframe.NewEmptyFrameSystem("transformFS") + executionFrame, err := referenceframe.New2DMobileModelFrame( + "ackframeExecutionFrame", ackermanFrame.DoF()[:3], roverGeom, + ) + test.That(t, err, test.ShouldBeNil) + fmt.Println("executionFrame.DoF(): ", executionFrame.DoF()) + fmt.Println("LEN executionFrame.DoF(): ", len(executionFrame.DoF())) + + err = tfFrameSystem.AddFrame(executionFrame, tfFrameSystem.World()) + test.That(t, err, test.ShouldBeNil) + + err = tfFrameSystem.MergeFrameSystem(fs, executionFrame) + test.That(t, err, test.ShouldBeNil) + inputs[executionFrame.Name()] = referenceframe.FloatsToInputs(make([]float64, 3)) t.Run("base case - validate plan without obstacles", func(t *testing.T) { - err := CheckPlan(ackermanFrame, plan, 0, nil, fs, startPose, inputs, errorState, math.Inf(1), logger) + // here we do not even need to create any state constraints so the test passes automatically when we reach + // CheckStateConstraints. + err := CheckPlan(ackermanFrame, plan, 0, nil, tfFrameSystem, startPose, inputs, errorState, math.Inf(1), logger) test.That(t, err, test.ShouldBeNil) }) @@ -301,7 +322,7 @@ func TestPtgCheckPlan(t *testing.T) { worldState, err := referenceframe.NewWorldState(gifs, nil) test.That(t, err, test.ShouldBeNil) - err = CheckPlan(ackermanFrame, plan, 0, worldState, fs, startPose, inputs, errorState, math.Inf(1), logger) + err = CheckPlan(ackermanFrame, plan, 0, worldState, tfFrameSystem, startPose, inputs, errorState, math.Inf(1), logger) test.That(t, err, test.ShouldNotBeNil) }) @@ -338,10 +359,14 @@ func TestPtgCheckPlan(t *testing.T) { worldState, err := referenceframe.NewWorldState(gifs, nil) test.That(t, err, test.ShouldBeNil) - err = CheckPlan(ackermanFrame, plan, 1, worldState, fs, startPose, inputs, errorState, math.Inf(1), logger) + // double check these inputs, the theta value might be wrong + inputs[executionFrame.Name()] = referenceframe.FloatsToInputs([]float64{2400, 0}) + err = CheckPlan(ackermanFrame, plan, 1, worldState, tfFrameSystem, startPose, inputs, errorState, 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) { + // i think we need to add an execution frame here???? obstacle, err := spatialmath.NewBox( spatialmath.NewPoseFromPoint(r3.Vector{2500, 20, 0}), r3.Vector{10, 2000, 1}, "obstacle", @@ -353,51 +378,54 @@ func TestPtgCheckPlan(t *testing.T) { worldState, err := referenceframe.NewWorldState(gifs, nil) test.That(t, err, test.ShouldBeNil) - err = CheckPlan(ackermanFrame, plan, 1, worldState, fs, startPose, inputs, errorState, math.Inf(1), logger) + // need to re-understand this test, probably some comment so that this is easier to reason about + err = CheckPlan(ackermanFrame, plan, 1, worldState, tfFrameSystem, startPose, plan.Trajectory()[1], errorState, 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) - - err = CheckPlan(ackermanFrame, plan, 2, worldState, fs, startPose, inputs, errorState, 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()) - errorState := spatialmath.PoseDelta(pathPose, startPose) - - err = CheckPlan(ackermanFrame, plan, 2, worldState, fs, startPose, inputs, errorState, math.Inf(1), logger) - 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) + + // 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) + + // err = CheckPlan(ackermanFrame, plan, 2, worldState, tfFrameSystem, startPose, inputs, errorState, 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()) + // errorState := spatialmath.PoseDelta(pathPose, startPose) + + // err = CheckPlan(ackermanFrame, plan, 2, worldState, tfFrameSystem, startPose, inputs, errorState, math.Inf(1), logger) + // test.That(t, err, test.ShouldBeNil) + // }) } func planToTpspaceRec(plan Plan, f referenceframe.Frame) ([]node, error) { diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 7533c8a9ac6..865539bf93a 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -1340,14 +1340,10 @@ func TestMoveOnMapStaticObs(t *testing.T) { }, nil, ) test.That(t, err, test.ShouldBeNil) - fmt.Println("PRINTING mr.absoluteFS.FrameNames()") - for _, name := range mr.absoluteFS.FrameNames() { - fmt.Println("name: ", name) - } + currentInputs := referenceframe.StartPositions(mr.absoluteFS) currentInputs["test-baseExecutionFrame"] = referenceframe.FloatsToInputs([]float64{0.58772e3, -0.80826e3, 0}) - fmt.Println("currentInputs :", currentInputs) err = motionplan.CheckPlan( mr.planRequest.Frame, plan, From ce5a1223429f7be0eb6115158a330f6fc6f1380e Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 3 May 2024 00:40:49 -0400 Subject: [PATCH 024/126] remove un-needed changes --- motionplan/constraint.go | 4 +++- motionplan/motionPlanner.go | 7 +++---- motionplan/solverFrame.go | 4 +--- 3 files changed, 7 insertions(+), 8 deletions(-) diff --git a/motionplan/constraint.go b/motionplan/constraint.go index 91612049755..caa13ee1038 100644 --- a/motionplan/constraint.go +++ b/motionplan/constraint.go @@ -241,7 +241,6 @@ func createAllCollisionConstraints( var err error if len(worldGeometries) > 0 { - // Check if a moving geometry is in collision with a pointcloud. If so, error. // TODO: This is not the most robust way to deal with this but is better than driving through walls. var zeroCG *collisionGraph @@ -263,6 +262,7 @@ func createAllCollisionConstraints( } } } + // create constraint to keep moving geometries from hitting world state obstacles obstacleConstraint, err := NewCollisionConstraint(movingRobotGeometries, worldGeometries, allowedCollisions, false, collisionBufferMM) if err != nil { @@ -337,6 +337,8 @@ func NewCollisionConstraint( } internalGeoms = internal.Geometries() case state.Position != nil: + // If we didn't pass a Configuration, but we do have a Position, then get the geometries at the zero state and + // transform them to the Position internal, err := state.Frame.Geometries(make([]referenceframe.Input, len(state.Frame.DoF()))) if err != nil { return false diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 777d2f1326c..3e03a0cbfff 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -483,13 +483,12 @@ func CheckPlan( if err != nil { return err } - plannerSetUpInputs := currentInputs - plannerSetUpInputs[checkFrame.Name()] = make([]frame.Input, len(checkFrame.DoF())) + // setup the planOpts if sfPlanner.planOpts, err = sfPlanner.plannerSetupFromMoveRequest( currentPose, poses[len(poses)-1], - plannerSetUpInputs, + currentInputs, worldState, nil, // no pb.Constraints nil, // no plannOpts @@ -522,7 +521,6 @@ func CheckPlan( currPose, nextPose spatialmath.Pose, currInput, nextInput map[string][]frame.Input, ) (*ik.Segment, error) { - // there will be an issue here since currInputs is not of the correct length currInputSlice, err := sf.mapToSlice(currInput) if err != nil { return nil, err @@ -582,6 +580,7 @@ func CheckPlan( if err != nil { return err } + // Check if look ahead distance has been reached currentTravelDistanceMM := totalTravelDistanceMM + poseInPath.Point().Distance(segment.StartPosition.Point()) if currentTravelDistanceMM > lookAheadDistanceMM { diff --git a/motionplan/solverFrame.go b/motionplan/solverFrame.go index cacd2d81a77..ddda64d6071 100644 --- a/motionplan/solverFrame.go +++ b/motionplan/solverFrame.go @@ -49,7 +49,6 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, if solveFrame == nil { return nil, frame.NewFrameMissingError(solveFrameName) } - solveFrameList, err := fs.TracebackFrame(solveFrame) if err != nil { return nil, err @@ -218,7 +217,6 @@ func (sf *solverFrame) Interpolate(from, to []frame.Input, by float64) ([]frame. interp := make([]frame.Input, 0, len(to)) posIdx := 0 for _, currFrame := range sf.frames { - // if we are dealing with the execution frame, no need to interpolate, just return what we got dof := len(currFrame.DoF()) + posIdx fromSubset := from[posIdx:dof] toSubset := to[posIdx:dof] @@ -228,6 +226,7 @@ func (sf *solverFrame) Interpolate(from, to []frame.Input, by float64) ([]frame. fmt.Println("currFrame.DoF()", currFrame.DoF()) fmt.Println("currFrame.Name()", currFrame.Name()) + // if we are dealing with the execution frame, no need to interpolate, just return what we got if strings.Contains(currFrame.Name(), "ExecutionFrame") { interp = append(interp, fromSubset...) fmt.Println("appending from...: ", fromSubset) @@ -281,7 +280,6 @@ func (sf *solverFrame) Geometries(inputs []frame.Input) (*frame.GeometriesInFram var errAll error inputMap := sf.sliceToMap(inputs) sfGeometries := []spatial.Geometry{} - for _, fName := range sf.movingFS.FrameNames() { if strings.Contains(fName, "ExecutionFrame") { continue From 1d18bb5d26cd84bc83c00827fea7e72d4a4379e0 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 3 May 2024 00:59:58 -0400 Subject: [PATCH 025/126] clean prints - TestPtgCheckPlan WIP --- motionplan/constraint.go | 1 - motionplan/motionPlanner.go | 7 ++++++- motionplan/planManager.go | 2 -- motionplan/solverFrame.go | 16 ++++++++-------- motionplan/tpSpaceRRT_test.go | 2 -- 5 files changed, 14 insertions(+), 14 deletions(-) diff --git a/motionplan/constraint.go b/motionplan/constraint.go index caa13ee1038..e0253007c39 100644 --- a/motionplan/constraint.go +++ b/motionplan/constraint.go @@ -291,7 +291,6 @@ func createAllCollisionConstraints( return nil, err } constraintMap[defaultSelfCollisionConstraintDesc] = selfCollisionConstraint - fmt.Println("constraintMap: ", constraintMap) } return constraintMap, nil } diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 3e03a0cbfff..4c213dfbd28 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -547,6 +547,9 @@ func CheckPlan( for i := wayPointIdx; i < len(offsetPlan.Path())-1; i++ { currInput := offsetPlan.Trajectory()[i] nextInput := offsetPlan.Trajectory()[i+1] + fmt.Println("poses[i]: ", spatialmath.PoseToProtobuf(poses[i])) + fmt.Println("poses[i+1]: ", spatialmath.PoseToProtobuf(poses[i+1])) + fmt.Println(" ") if relative { currInput[checkFrame.Name()+"ExecutionFrame"] = frame.FloatsToInputs( []float64{poses[i].Point().X, poses[i].Point().Y, poses[i].Orientation().OrientationVectorRadians().Theta}, @@ -563,7 +566,7 @@ func CheckPlan( } for _, s := range segments { - fmt.Println("segment: ", s) + fmt.Println(s) } // go through segments and check that we satisfy constraints @@ -575,6 +578,7 @@ func CheckPlan( if err != nil { return err } + fmt.Println("len(interpolatedConfigurations): ", len(interpolatedConfigurations)) for _, interpConfig := range interpolatedConfigurations { poseInPath, err := sf.Transform(interpConfig) if err != nil { @@ -595,6 +599,7 @@ func CheckPlan( return err } fmt.Println("sfTfPose: ", spatialmath.PoseToProtobuf(sfTfPose)) + fmt.Println(" ") // 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 { diff --git a/motionplan/planManager.go b/motionplan/planManager.go index 180c259d3a5..6f196aa9741 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -541,8 +541,6 @@ func (pm *planManager) plannerSetupFromMoveRequest( if err != nil { return nil, err } - fmt.Println("collisionConstraints: ", collisionConstraints) - fmt.Println("LEN collisionConstraints: ", len(collisionConstraints)) for name, constraint := range collisionConstraints { opt.AddStateConstraint(name, constraint) } diff --git a/motionplan/solverFrame.go b/motionplan/solverFrame.go index ddda64d6071..6545c4b18af 100644 --- a/motionplan/solverFrame.go +++ b/motionplan/solverFrame.go @@ -188,7 +188,7 @@ func (sf *solverFrame) Name() string { // Transform returns the pose between the two frames of this solver for a given set of inputs. func (sf *solverFrame) Transform(inputs []frame.Input) (spatial.Pose, error) { - fmt.Println("sf.DoF(): ", sf.DoF()) + // fmt.Println("sf.DoF(): ", sf.DoF()) fmt.Println("inputs: ", inputs) if len(inputs) != len(sf.DoF()) { return nil, frame.NewIncorrectInputLengthError(len(inputs), len(sf.DoF())) @@ -207,7 +207,7 @@ func (sf *solverFrame) Transform(inputs []frame.Input) (spatial.Pose, error) { // Interpolate interpolates the given amount between the two sets of inputs. func (sf *solverFrame) Interpolate(from, to []frame.Input, by float64) ([]frame.Input, error) { - fmt.Println("WITHIN SF INTERPOLATE") + // fmt.Println("WITHIN SF INTERPOLATE") if len(from) != len(sf.DoF()) { return nil, frame.NewIncorrectInputLengthError(len(from), len(sf.DoF())) } @@ -224,13 +224,13 @@ func (sf *solverFrame) Interpolate(from, to []frame.Input, by float64) ([]frame. var interpSub []frame.Input var err error - fmt.Println("currFrame.DoF()", currFrame.DoF()) - fmt.Println("currFrame.Name()", currFrame.Name()) + // fmt.Println("currFrame.DoF()", currFrame.DoF()) + // fmt.Println("currFrame.Name()", currFrame.Name()) // if we are dealing with the execution frame, no need to interpolate, just return what we got if strings.Contains(currFrame.Name(), "ExecutionFrame") { interp = append(interp, fromSubset...) - fmt.Println("appending from...: ", fromSubset) - fmt.Println("interp: ", interp) + // fmt.Println("appending from...: ", fromSubset) + // fmt.Println("interp: ", interp) continue } interpSub, err = currFrame.Interpolate(fromSubset, toSubset, by) @@ -239,8 +239,8 @@ func (sf *solverFrame) Interpolate(from, to []frame.Input, by float64) ([]frame. } interp = append(interp, interpSub...) - fmt.Println("appending from...: ", interpSub) - fmt.Println("interp: ", interpSub) + // fmt.Println("appending from...: ", interpSub) + // fmt.Println("interp: ", interpSub) } return interp, nil } diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index 3d4792fa6ed..37eb611d81b 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -295,8 +295,6 @@ func TestPtgCheckPlan(t *testing.T) { "ackframeExecutionFrame", ackermanFrame.DoF()[:3], roverGeom, ) test.That(t, err, test.ShouldBeNil) - fmt.Println("executionFrame.DoF(): ", executionFrame.DoF()) - fmt.Println("LEN executionFrame.DoF(): ", len(executionFrame.DoF())) err = tfFrameSystem.AddFrame(executionFrame, tfFrameSystem.World()) test.That(t, err, test.ShouldBeNil) From 2570e15e35199a9e46e4358fbc273bed136100f2 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 3 May 2024 13:25:38 -0400 Subject: [PATCH 026/126] tpspace rrt test testptgcheckplan passes --- motionplan/motionPlanner.go | 26 +++++++-- motionplan/tpSpaceRRT_test.go | 100 +++++++++++++++++----------------- 2 files changed, 72 insertions(+), 54 deletions(-) diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 4c213dfbd28..51f49cd69e8 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -483,7 +483,7 @@ func CheckPlan( if err != nil { return err } - + fmt.Println("currentInputs: ", currentInputs) // setup the planOpts if sfPlanner.planOpts, err = sfPlanner.plannerSetupFromMoveRequest( currentPose, @@ -493,6 +493,7 @@ func CheckPlan( nil, // no pb.Constraints nil, // no plannOpts ); err != nil { + fmt.Println(err) return err } @@ -505,6 +506,11 @@ func CheckPlan( if err != nil { return err } + fmt.Println("START CFG checkFrameCurrentInputs: ", checkFrameCurrentInputs) + fmt.Println("END CFG checkFrameCurrentInputs: ", checkFrameCurrentInputs) + fmt.Println("StartPosition: ", spatialmath.PoseToProtobuf(currentPose)) + fmt.Println("EndPosition: ", spatialmath.PoseToProtobuf(poses[wayPointIdx])) + fmt.Println(" ") // pre-pend to segments so we can connect to the input we have not finished actuating yet segments = append(segments, &ik.Segment{ @@ -521,6 +527,10 @@ func CheckPlan( currPose, nextPose spatialmath.Pose, currInput, nextInput map[string][]frame.Input, ) (*ik.Segment, error) { + fmt.Println("currInput: ", currInput) + fmt.Println("nextInput: ", nextInput) + fmt.Println("currPose: ", spatialmath.PoseToProtobuf(currPose)) + fmt.Println("nextPose: ", spatialmath.PoseToProtobuf(nextPose)) currInputSlice, err := sf.mapToSlice(currInput) if err != nil { return nil, err @@ -547,9 +557,6 @@ func CheckPlan( for i := wayPointIdx; i < len(offsetPlan.Path())-1; i++ { currInput := offsetPlan.Trajectory()[i] nextInput := offsetPlan.Trajectory()[i+1] - fmt.Println("poses[i]: ", spatialmath.PoseToProtobuf(poses[i])) - fmt.Println("poses[i+1]: ", spatialmath.PoseToProtobuf(poses[i+1])) - fmt.Println(" ") if relative { currInput[checkFrame.Name()+"ExecutionFrame"] = frame.FloatsToInputs( []float64{poses[i].Point().X, poses[i].Point().Y, poses[i].Orientation().OrientationVectorRadians().Theta}, @@ -557,17 +564,28 @@ func CheckPlan( nextInput[checkFrame.Name()+"ExecutionFrame"] = frame.FloatsToInputs( []float64{poses[i+1].Point().X, poses[i+1].Point().Y, poses[i+1].Orientation().OrientationVectorRadians().Theta}, ) + if i == 0 { + currInput = nextInput + currInput[checkFrame.Name()+"ExecutionFrame"] = frame.FloatsToInputs( + []float64{poses[i].Point().X, poses[i].Point().Y, poses[i].Orientation().OrientationVectorRadians().Theta}, + ) + } } segment, err := createSegment(poses[i], poses[i+1], currInput, nextInput) if err != nil { return err } + fmt.Println("created the following segment : ", segment) + fmt.Println(" ") segments = append(segments, segment) } for _, s := range segments { fmt.Println(s) + fmt.Println(" ") } + fmt.Println(" ") + fmt.Println(" ") // 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 diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index 37eb611d81b..cb04bb29972 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -319,7 +319,6 @@ func TestPtgCheckPlan(t *testing.T) { worldState, err := referenceframe.NewWorldState(gifs, nil) test.That(t, err, test.ShouldBeNil) - err = CheckPlan(ackermanFrame, plan, 0, worldState, tfFrameSystem, startPose, inputs, errorState, math.Inf(1), logger) test.That(t, err, test.ShouldNotBeNil) }) @@ -327,7 +326,7 @@ func TestPtgCheckPlan(t *testing.T) { // 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) + err = tfFrameSystem.AddFrame(cameraOriginFrame, ackermanFrame) test.That(t, err, test.ShouldBeNil) // create camera geometry @@ -342,7 +341,7 @@ func TestPtgCheckPlan(t *testing.T) { "camera-frame", spatialmath.NewPoseFromPoint(r3.Vector{0, 0, 0}), cameraGeom, ) test.That(t, err, test.ShouldBeNil) - err = fs.AddFrame(cameraFrame, cameraOriginFrame) + err = tfFrameSystem.AddFrame(cameraFrame, cameraOriginFrame) test.That(t, err, test.ShouldBeNil) t.Run("obstacles NOT in world frame - no collision - integration test", func(t *testing.T) { @@ -357,14 +356,15 @@ func TestPtgCheckPlan(t *testing.T) { worldState, err := referenceframe.NewWorldState(gifs, nil) test.That(t, err, test.ShouldBeNil) + fmt.Println("tfFrameSystem.FrameNames(): ", tfFrameSystem.FrameNames()) + // double check these inputs, the theta value might be wrong - inputs[executionFrame.Name()] = referenceframe.FloatsToInputs([]float64{2400, 0}) + inputs[executionFrame.Name()] = referenceframe.FloatsToInputs([]float64{2331.83, 14, 0}) err = CheckPlan(ackermanFrame, plan, 1, worldState, tfFrameSystem, startPose, inputs, errorState, 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) { - // i think we need to add an execution frame here???? obstacle, err := spatialmath.NewBox( spatialmath.NewPoseFromPoint(r3.Vector{2500, 20, 0}), r3.Vector{10, 2000, 1}, "obstacle", @@ -376,54 +376,54 @@ func TestPtgCheckPlan(t *testing.T) { worldState, err := referenceframe.NewWorldState(gifs, nil) test.That(t, err, test.ShouldBeNil) - // need to re-understand this test, probably some comment so that this is easier to reason about - err = CheckPlan(ackermanFrame, plan, 1, worldState, tfFrameSystem, startPose, plan.Trajectory()[1], errorState, math.Inf(1), logger) + inputs[executionFrame.Name()] = referenceframe.FloatsToInputs([]float64{2331.83, 14, 0}) + fmt.Println("inputs: ", inputs) + inputs[ackermanFrame.Name()] = referenceframe.FloatsToInputs([]float64{0, 0, 0, 0}) + + err = CheckPlan(ackermanFrame, plan, 1, worldState, tfFrameSystem, startPose, inputs, errorState, 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) - - // err = CheckPlan(ackermanFrame, plan, 2, worldState, tfFrameSystem, startPose, inputs, errorState, 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()) - // errorState := spatialmath.PoseDelta(pathPose, startPose) - - // err = CheckPlan(ackermanFrame, plan, 2, worldState, tfFrameSystem, startPose, inputs, errorState, math.Inf(1), logger) - // 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) + + 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) + + err = CheckPlan(ackermanFrame, plan, 2, worldState, tfFrameSystem, startPose, inputs, errorState, 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) + + pathPose := plan.Path()[2][ackermanFrame.Name()].Pose() + startPose := spatialmath.NewPose(r3.Vector{2499, 0, 0}, pathPose.Orientation()) + errorState := spatialmath.PoseDelta(pathPose, startPose) + + err = CheckPlan(ackermanFrame, plan, 2, worldState, tfFrameSystem, startPose, inputs, errorState, math.Inf(1), logger) + test.That(t, err, test.ShouldBeNil) + }) } func planToTpspaceRec(plan Plan, f referenceframe.Frame) ([]node, error) { From 4b42874032ab90d6cab417dcbf19be4863854ed2 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 3 May 2024 13:26:48 -0400 Subject: [PATCH 027/126] tpspace rrt test testptgcheckplan passes --- motionplan/motionPlanner.go | 28 +--------------------------- motionplan/solverFrame.go | 10 ---------- 2 files changed, 1 insertion(+), 37 deletions(-) diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 51f49cd69e8..6d4d12f7527 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -483,7 +483,7 @@ func CheckPlan( if err != nil { return err } - fmt.Println("currentInputs: ", currentInputs) + // setup the planOpts if sfPlanner.planOpts, err = sfPlanner.plannerSetupFromMoveRequest( currentPose, @@ -493,7 +493,6 @@ func CheckPlan( nil, // no pb.Constraints nil, // no plannOpts ); err != nil { - fmt.Println(err) return err } @@ -506,11 +505,6 @@ func CheckPlan( if err != nil { return err } - fmt.Println("START CFG checkFrameCurrentInputs: ", checkFrameCurrentInputs) - fmt.Println("END CFG checkFrameCurrentInputs: ", checkFrameCurrentInputs) - fmt.Println("StartPosition: ", spatialmath.PoseToProtobuf(currentPose)) - fmt.Println("EndPosition: ", spatialmath.PoseToProtobuf(poses[wayPointIdx])) - fmt.Println(" ") // pre-pend to segments so we can connect to the input we have not finished actuating yet segments = append(segments, &ik.Segment{ @@ -527,10 +521,6 @@ func CheckPlan( currPose, nextPose spatialmath.Pose, currInput, nextInput map[string][]frame.Input, ) (*ik.Segment, error) { - fmt.Println("currInput: ", currInput) - fmt.Println("nextInput: ", nextInput) - fmt.Println("currPose: ", spatialmath.PoseToProtobuf(currPose)) - fmt.Println("nextPose: ", spatialmath.PoseToProtobuf(nextPose)) currInputSlice, err := sf.mapToSlice(currInput) if err != nil { return nil, err @@ -575,18 +565,9 @@ func CheckPlan( if err != nil { return err } - fmt.Println("created the following segment : ", segment) - fmt.Println(" ") segments = append(segments, segment) } - for _, s := range segments { - fmt.Println(s) - fmt.Println(" ") - } - fmt.Println(" ") - fmt.Println(" ") - // 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. @@ -596,7 +577,6 @@ func CheckPlan( if err != nil { return err } - fmt.Println("len(interpolatedConfigurations): ", len(interpolatedConfigurations)) for _, interpConfig := range interpolatedConfigurations { poseInPath, err := sf.Transform(interpConfig) if err != nil { @@ -612,12 +592,6 @@ func CheckPlan( // define State which only houses inputs, pose information not needed interpolatedState := &ik.State{Frame: sf} interpolatedState.Configuration = interpConfig - sfTfPose, err := sf.Transform(interpConfig) - if err != nil { - return err - } - fmt.Println("sfTfPose: ", spatialmath.PoseToProtobuf(sfTfPose)) - fmt.Println(" ") // 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 { diff --git a/motionplan/solverFrame.go b/motionplan/solverFrame.go index 6545c4b18af..c3145977c42 100644 --- a/motionplan/solverFrame.go +++ b/motionplan/solverFrame.go @@ -2,7 +2,6 @@ package motionplan import ( "errors" - "fmt" "strings" "go.uber.org/multierr" @@ -188,8 +187,6 @@ func (sf *solverFrame) Name() string { // Transform returns the pose between the two frames of this solver for a given set of inputs. func (sf *solverFrame) Transform(inputs []frame.Input) (spatial.Pose, error) { - // fmt.Println("sf.DoF(): ", sf.DoF()) - fmt.Println("inputs: ", inputs) if len(inputs) != len(sf.DoF()) { return nil, frame.NewIncorrectInputLengthError(len(inputs), len(sf.DoF())) } @@ -207,7 +204,6 @@ func (sf *solverFrame) Transform(inputs []frame.Input) (spatial.Pose, error) { // Interpolate interpolates the given amount between the two sets of inputs. func (sf *solverFrame) Interpolate(from, to []frame.Input, by float64) ([]frame.Input, error) { - // fmt.Println("WITHIN SF INTERPOLATE") if len(from) != len(sf.DoF()) { return nil, frame.NewIncorrectInputLengthError(len(from), len(sf.DoF())) } @@ -224,13 +220,9 @@ func (sf *solverFrame) Interpolate(from, to []frame.Input, by float64) ([]frame. var interpSub []frame.Input var err error - // fmt.Println("currFrame.DoF()", currFrame.DoF()) - // fmt.Println("currFrame.Name()", currFrame.Name()) // if we are dealing with the execution frame, no need to interpolate, just return what we got if strings.Contains(currFrame.Name(), "ExecutionFrame") { interp = append(interp, fromSubset...) - // fmt.Println("appending from...: ", fromSubset) - // fmt.Println("interp: ", interp) continue } interpSub, err = currFrame.Interpolate(fromSubset, toSubset, by) @@ -239,8 +231,6 @@ func (sf *solverFrame) Interpolate(from, to []frame.Input, by float64) ([]frame. } interp = append(interp, interpSub...) - // fmt.Println("appending from...: ", interpSub) - // fmt.Println("interp: ", interpSub) } return interp, nil } From a4d816f4ac906d83f3e64d58c356794901863542 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 3 May 2024 13:56:59 -0400 Subject: [PATCH 028/126] linted --- motionplan/constraint.go | 1 - motionplan/tpSpaceRRT_test.go | 6 ------ protoutils/messages.go | 1 - services/motion/builtin/builtin_test.go | 1 - services/motion/builtin/move_request.go | 4 +++- 5 files changed, 3 insertions(+), 10 deletions(-) diff --git a/motionplan/constraint.go b/motionplan/constraint.go index e0253007c39..0d11effd224 100644 --- a/motionplan/constraint.go +++ b/motionplan/constraint.go @@ -251,7 +251,6 @@ func createAllCollisionConstraints( if err != nil { return nil, err } - } for _, collision := range zeroCG.collisions(collisionBufferMM) { if collision.name1 == octree.Label() { diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index cb04bb29972..8050958ef78 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -4,7 +4,6 @@ package motionplan import ( "context" - "fmt" "math" "math/rand" "testing" @@ -286,8 +285,6 @@ func TestPtgCheckPlan(t *testing.T) { startPose := spatialmath.NewPoseFromPoint(r3.Vector{0, 0, 0}) errorState := startPose inputs := plan.Trajectory()[0] - fmt.Println("plan.Trajectory(): ", plan.Trajectory()) - fmt.Println("plan.Path(): ", plan.Path()) // NOTE: WE NEED TO ADD AN EXECUTION FRAME TO THE CHECKING FRAMESYSTEM SINCE WE ONLY WANT TO RELY ON USING INPUTS tfFrameSystem := referenceframe.NewEmptyFrameSystem("transformFS") @@ -356,8 +353,6 @@ func TestPtgCheckPlan(t *testing.T) { worldState, err := referenceframe.NewWorldState(gifs, nil) test.That(t, err, test.ShouldBeNil) - fmt.Println("tfFrameSystem.FrameNames(): ", tfFrameSystem.FrameNames()) - // double check these inputs, the theta value might be wrong inputs[executionFrame.Name()] = referenceframe.FloatsToInputs([]float64{2331.83, 14, 0}) err = CheckPlan(ackermanFrame, plan, 1, worldState, tfFrameSystem, startPose, inputs, errorState, math.Inf(1), logger) @@ -377,7 +372,6 @@ func TestPtgCheckPlan(t *testing.T) { test.That(t, err, test.ShouldBeNil) inputs[executionFrame.Name()] = referenceframe.FloatsToInputs([]float64{2331.83, 14, 0}) - fmt.Println("inputs: ", inputs) inputs[ackermanFrame.Name()] = referenceframe.FloatsToInputs([]float64{0, 0, 0, 0}) err = CheckPlan(ackermanFrame, plan, 1, worldState, tfFrameSystem, startPose, inputs, errorState, math.Inf(1), logger) diff --git a/protoutils/messages.go b/protoutils/messages.go index e86da1e9340..98808062150 100644 --- a/protoutils/messages.go +++ b/protoutils/messages.go @@ -6,7 +6,6 @@ import ( "strconv" "github.com/golang/geo/r3" - //nolint:staticcheck protov1 "github.com/golang/protobuf/proto" commonpb "go.viam.com/api/common/v1" "go.viam.com/utils/protoutils" diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 865539bf93a..5def9485c66 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -2636,7 +2636,6 @@ func TestGetTransientDetections(t *testing.T) { 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) }) } diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index bafdddb8104..126a894abbd 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -306,7 +306,9 @@ func (mr *moveRequest) obstaclesIntersectPlan( return state.ExecuteResponse{}, nil } -func (mr *moveRequest) getCurrentInputsAndPosition(ctx context.Context) (map[string][]referenceframe.Input, *referenceframe.PoseInFrame, error) { +func (mr *moveRequest) getCurrentInputsAndPosition( + ctx context.Context, +) (map[string][]referenceframe.Input, *referenceframe.PoseInFrame, error) { currentInputs := referenceframe.StartPositions(mr.absoluteFS) pif, err := mr.kinematicBase.CurrentPosition(ctx) if err != nil { From fff9cf64675de34c2cc66fb9a0ebf0136a5df352 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 3 May 2024 14:05:58 -0400 Subject: [PATCH 029/126] singlar bone of AddFrameBetween + revert messages.go since we are not touching that --- protoutils/messages.go | 1 + referenceframe/frame_system.go | 4 ++++ 2 files changed, 5 insertions(+) diff --git a/protoutils/messages.go b/protoutils/messages.go index 98808062150..e86da1e9340 100644 --- a/protoutils/messages.go +++ b/protoutils/messages.go @@ -6,6 +6,7 @@ import ( "strconv" "github.com/golang/geo/r3" + //nolint:staticcheck protov1 "github.com/golang/protobuf/proto" commonpb "go.viam.com/api/common/v1" "go.viam.com/utils/protoutils" diff --git a/referenceframe/frame_system.go b/referenceframe/frame_system.go index cf25296fb3a..121a89f3086 100644 --- a/referenceframe/frame_system.go +++ b/referenceframe/frame_system.go @@ -719,3 +719,7 @@ func poseFromPositions(frame Frame, positions map[string][]Input) (spatial.Pose, } return frame.Transform(inputs) } + +func AddFrameBetween() { + +} From 074cfbbe442a4343cd27594372f3e020d2fbb0f9 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 3 May 2024 14:29:14 -0400 Subject: [PATCH 030/126] wip change to pass lint --- referenceframe/frame_system.go | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/referenceframe/frame_system.go b/referenceframe/frame_system.go index 121a89f3086..04f5624d9fc 100644 --- a/referenceframe/frame_system.go +++ b/referenceframe/frame_system.go @@ -720,6 +720,6 @@ func poseFromPositions(frame Frame, positions map[string][]Input) (spatial.Pose, return frame.Transform(inputs) } -func AddFrameBetween() { +// func AddFrameBetween() { -} +// } From c745c4bdc52788a438e34e632ad7eaa8847cb2de Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 3 May 2024 15:07:00 -0400 Subject: [PATCH 031/126] fix ptgkinematics no geom tests + add AddFrameBetween --- motionplan/planManager.go | 2 +- referenceframe/frame_system.go | 24 ++++++++++++++---------- 2 files changed, 15 insertions(+), 11 deletions(-) diff --git a/motionplan/planManager.go b/motionplan/planManager.go index 6f196aa9741..1e5d623e784 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -795,7 +795,7 @@ func (pm *planManager) planRelativeWaypoint(ctx context.Context, request *PlanRe // anywhere else but the origin copyOfOriginalFS := pm.frame.fss - transformFrame, err := referenceframe.NewStaticFrame("", request.StartPose) + transformFrame, err := referenceframe.NewStaticFrame(request.Frame.Name()+"ExecutionFrame", request.StartPose) if err != nil { return nil, err } diff --git a/referenceframe/frame_system.go b/referenceframe/frame_system.go index 04f5624d9fc..b9950a9a871 100644 --- a/referenceframe/frame_system.go +++ b/referenceframe/frame_system.go @@ -68,13 +68,6 @@ type FrameSystem interface { // frame's children and parentage to replacementFrame. The original frame is removed entirely from the frame system. // replacementFrame is not allowed to exist within the frame system at the time of the call. ReplaceFrame(replacementFrame Frame) error - - // consider adding method `AddFrameBetween` to this interface for ease of adding a static transform between the - // kb planning frame and the world when it comes to planning - // also when it comed to adding a execution frame between the execution frame - // new: should not be a method on the interface but should instead just - // exist as a helper function which returns an augmented copy of the - // pass in framesystem } // FrameSystemPart is used to collect all the info need from a named robot part to build the frame node in a frame system. @@ -720,6 +713,17 @@ func poseFromPositions(frame Frame, positions map[string][]Input) (spatial.Pose, return frame.Transform(inputs) } -// func AddFrameBetween() { - -// } +// AddFrameBetween takes an existing framesystem which has general form: +// originalFS := world - f1 - f2 - ... +// and places the middle frame between f1 and world resulting in +// newFS := world - middle - f1 - f2 ... +func AddFrameBetween(middle Frame, originalFS FrameSystem) (FrameSystem, error) { + newFS := NewEmptyFrameSystem("with-in-between Frame") + if err := newFS.AddFrame(middle, newFS.World()); err != nil { + return nil, err + } + if err := newFS.MergeFrameSystem(originalFS, middle); err != nil { + return nil, err + } + return newFS, nil +} From a21cc9904bc3ab6c58cf835f86e61912bd85b6c4 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 6 May 2024 12:20:46 -0400 Subject: [PATCH 032/126] fix TestMoveOnMapStaticObs --- motionplan/motionPlanner.go | 8 +++----- services/motion/builtin/builtin_test.go | 18 ++++++++---------- 2 files changed, 11 insertions(+), 15 deletions(-) diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 6d4d12f7527..e9a8734871d 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -552,13 +552,10 @@ func CheckPlan( []float64{poses[i].Point().X, poses[i].Point().Y, poses[i].Orientation().OrientationVectorRadians().Theta}, ) nextInput[checkFrame.Name()+"ExecutionFrame"] = frame.FloatsToInputs( - []float64{poses[i+1].Point().X, poses[i+1].Point().Y, poses[i+1].Orientation().OrientationVectorRadians().Theta}, + []float64{poses[i].Point().X, poses[i].Point().Y, poses[i].Orientation().OrientationVectorRadians().Theta}, ) if i == 0 { currInput = nextInput - currInput[checkFrame.Name()+"ExecutionFrame"] = frame.FloatsToInputs( - []float64{poses[i].Point().X, poses[i].Point().Y, poses[i].Orientation().OrientationVectorRadians().Theta}, - ) } } segment, err := createSegment(poses[i], poses[i+1], currInput, nextInput) @@ -589,7 +586,8 @@ func CheckPlan( return nil } - // define State which only houses inputs, pose information not needed + // define State which only houses inputs, pose information not needed since we cannot get arcs from + // an interpolating poses, this would only yeild a straight line. interpolatedState := &ik.State{Frame: sf} interpolatedState.Configuration = interpConfig diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 5def9485c66..8bdfc9126c9 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -1240,7 +1240,6 @@ func TestMoveOnMapStaticObs(t *testing.T) { extra := map[string]interface{}{ "motion_profile": "position_only", "timeout": 5., - "smooth_iter": 10., } baseName := "test-base" @@ -1300,13 +1299,6 @@ func TestMoveOnMapStaticObs(t *testing.T) { // robot's position across the path. By showing that we have a collision on the path with an // obstacle on the left we prove that our path does not collide with the original obstacle // placed on the right. - obstacleLeft, err := spatialmath.NewBox( - spatialmath.NewPose(r3.Vector{X: 0.22981e3, Y: -0.38875e3, Z: 0}, - &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 45}), - r3.Vector{X: 900, Y: 10, Z: 10}, - "obstacleLeft", - ) - test.That(t, err, test.ShouldBeNil) obstacleRight, err := spatialmath.NewBox( spatialmath.NewPose(r3.Vector{0.89627e3, -0.37192e3, 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: -45}), @@ -1330,7 +1322,13 @@ func TestMoveOnMapStaticObs(t *testing.T) { // place obstacle in opposite position and show that the generate path // collides with obstacleLeft - + obstacleLeft, err := spatialmath.NewBox( + spatialmath.NewPose(r3.Vector{X: 0.22981e3, Y: -0.38875e3, Z: 0}, + &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 45}), + r3.Vector{X: 900, Y: 10, Z: 10}, + "obstacleLeft", + ) + test.That(t, err, test.ShouldBeNil) wrldSt, err := referenceframe.NewWorldState( []*referenceframe.GeometriesInFrame{ referenceframe.NewGeometriesInFrame( @@ -1347,7 +1345,7 @@ func TestMoveOnMapStaticObs(t *testing.T) { err = motionplan.CheckPlan( mr.planRequest.Frame, plan, - 1, + 0, wrldSt, mr.absoluteFS, spatialmath.NewPose( From c064634207d71139e94c1567595a2080d86003c0 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 6 May 2024 13:13:38 -0400 Subject: [PATCH 033/126] linted --- motionplan/motionPlanner.go | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index e9a8734871d..9cfbd3ba0b9 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -587,7 +587,7 @@ func CheckPlan( } // define State which only houses inputs, pose information not needed since we cannot get arcs from - // an interpolating poses, this would only yeild a straight line. + // an interpolating poses, this would only yield a straight line. interpolatedState := &ik.State{Frame: sf} interpolatedState.Configuration = interpConfig From e91f0a94465db02b0beb3f82378414897cffb369 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 6 May 2024 13:45:10 -0400 Subject: [PATCH 034/126] add debug log to see where this test is failing on CI --- motionplan/motionPlanner.go | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 9cfbd3ba0b9..1b698f5d895 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -590,6 +590,11 @@ func CheckPlan( // an interpolating poses, this would only yield a straight line. interpolatedState := &ik.State{Frame: sf} interpolatedState.Configuration = interpConfig + out, err := sf.Transform(interpConfig) + if err != nil { + return err + } + logger.Debugf("out: %v", spatialmath.PoseToProtobuf(out)) // 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 { @@ -601,6 +606,8 @@ func CheckPlan( } } + fmt.Println(" ") + // Update total traveled distance after segment has been checked totalTravelDistanceMM += segment.EndPosition.Point().Distance(segment.StartPosition.Point()) } From ac7d32aaba8b4c3942d1722a4de6a4faa499d733 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 6 May 2024 13:56:48 -0400 Subject: [PATCH 035/126] remove print so we pass linter --- motionplan/motionPlanner.go | 2 -- 1 file changed, 2 deletions(-) diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 1b698f5d895..9c0b5a89313 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -606,8 +606,6 @@ func CheckPlan( } } - fmt.Println(" ") - // Update total traveled distance after segment has been checked totalTravelDistanceMM += segment.EndPosition.Point().Distance(segment.StartPosition.Point()) } From 5bb3375e90ec670100f08bfc61873ba427d0462d Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 8 May 2024 15:58:48 -0400 Subject: [PATCH 036/126] remove need to check for 'ExecutionFrame' in solverFrame code --- components/base/kinematicbase/ptgKinematics.go | 9 +++++++-- motionplan/motionPlanner.go | 5 ----- motionplan/solverFrame.go | 10 +--------- 3 files changed, 8 insertions(+), 16 deletions(-) diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index f68f56f9d0c..8c803dced58 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -7,6 +7,7 @@ package kinematicbase import ( "context" "errors" + "math" "sync" "go.viam.com/rdk/components/base" @@ -136,8 +137,12 @@ func wrapWithPTGKinematics( // construct executionFrame executionFrame, err := referenceframe.New2DMobileModelFrame( b.Name().ShortName()+"ExecutionFrame", - planningFrame.DoF()[:3], - geometries[0], + []referenceframe.Limit{ + {Min: math.Inf(-1), Max: math.Inf(1)}, + {Min: math.Inf(-1), Max: math.Inf(1)}, + {Min: -360, Max: 360}, + }, + nil, ) if err != nil { return nil, err diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 9c0b5a89313..9cfbd3ba0b9 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -590,11 +590,6 @@ func CheckPlan( // an interpolating poses, this would only yield a straight line. interpolatedState := &ik.State{Frame: sf} interpolatedState.Configuration = interpConfig - out, err := sf.Transform(interpConfig) - if err != nil { - return err - } - logger.Debugf("out: %v", spatialmath.PoseToProtobuf(out)) // 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 { diff --git a/motionplan/solverFrame.go b/motionplan/solverFrame.go index c3145977c42..ed8dd8c5cb9 100644 --- a/motionplan/solverFrame.go +++ b/motionplan/solverFrame.go @@ -2,7 +2,6 @@ package motionplan import ( "errors" - "strings" "go.uber.org/multierr" pb "go.viam.com/api/component/arm/v1" @@ -52,6 +51,7 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, if err != nil { return nil, err } + // this should always include world if len(solveFrameList) == 0 { return nil, errors.New("solveFrameList was empty") } @@ -220,11 +220,6 @@ func (sf *solverFrame) Interpolate(from, to []frame.Input, by float64) ([]frame. var interpSub []frame.Input var err error - // if we are dealing with the execution frame, no need to interpolate, just return what we got - if strings.Contains(currFrame.Name(), "ExecutionFrame") { - interp = append(interp, fromSubset...) - continue - } interpSub, err = currFrame.Interpolate(fromSubset, toSubset, by) if err != nil { return nil, err @@ -271,9 +266,6 @@ func (sf *solverFrame) Geometries(inputs []frame.Input) (*frame.GeometriesInFram inputMap := sf.sliceToMap(inputs) sfGeometries := []spatial.Geometry{} for _, fName := range sf.movingFS.FrameNames() { - if strings.Contains(fName, "ExecutionFrame") { - continue - } f := sf.fss.Frame(fName) if f == nil { return nil, frame.NewFrameMissingError(fName) From eb5a38ac77f2fe2b2a2cef9c229f7d72af931f93 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 8 May 2024 17:15:48 -0400 Subject: [PATCH 037/126] fix CheckPlan --- components/base/kinematicbase/fake_kinematics.go | 2 +- motionplan/tpSpaceRRT_test.go | 8 ++++++-- referenceframe/frame.go | 1 + 3 files changed, 8 insertions(+), 3 deletions(-) diff --git a/components/base/kinematicbase/fake_kinematics.go b/components/base/kinematicbase/fake_kinematics.go index ec87af29d7c..4c318c44608 100644 --- a/components/base/kinematicbase/fake_kinematics.go +++ b/components/base/kinematicbase/fake_kinematics.go @@ -196,7 +196,7 @@ func WrapWithFakePTGKinematics( executionFrame, err := referenceframe.New2DMobileModelFrame( b.Name().ShortName()+"ExecutionFrame", planningFrame.DoF()[:3], - geometries[0], + nil, ) if err != nil { return nil, err diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index 8050958ef78..6b2533c898f 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -245,7 +245,7 @@ func TestTPsmoothing(t *testing.T) { func TestPtgCheckPlan(t *testing.T) { logger := logging.NewTestLogger(t) - roverGeom, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{10, 10, 10}, "") + roverGeom, err := spatialmath.NewBox(spatialmath.NewZeroPose(), r3.Vector{10, 10, 10}, "roverGeom") test.That(t, err, test.ShouldBeNil) geometries := []spatialmath.Geometry{roverGeom} ackermanFrame, err := tpspace.NewPTGFrameFromKinematicOptions( @@ -289,7 +289,11 @@ func TestPtgCheckPlan(t *testing.T) { // NOTE: WE NEED TO ADD AN EXECUTION FRAME TO THE CHECKING FRAMESYSTEM SINCE WE ONLY WANT TO RELY ON USING INPUTS tfFrameSystem := referenceframe.NewEmptyFrameSystem("transformFS") executionFrame, err := referenceframe.New2DMobileModelFrame( - "ackframeExecutionFrame", ackermanFrame.DoF()[:3], roverGeom, + "ackframeExecutionFrame", []referenceframe.Limit{ + {Min: math.Inf(-1), Max: math.Inf(1)}, + {Min: math.Inf(-1), Max: math.Inf(1)}, + {Min: -360, Max: 360}, + }, nil, ) test.That(t, err, test.ShouldBeNil) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 0fe4d65e9a3..8539832cf88 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -259,6 +259,7 @@ func (sf *staticFrame) ProtobufFromInput(input []Input) *pb.JointPositions { // Geometries returns an object representing the 3D space associeted with the staticFrame. func (sf *staticFrame) Geometries(input []Input) (*GeometriesInFrame, error) { + fmt.Println("static frame geometry: ", sf.geometry) if sf.geometry == nil { return NewGeometriesInFrame(sf.Name(), nil), nil } From d7e1a6b6f47dd9437ca51239e75735e02cd8b1ed Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 8 May 2024 17:17:05 -0400 Subject: [PATCH 038/126] sans fmt.Println in frame.go --- referenceframe/frame.go | 1 - 1 file changed, 1 deletion(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 8539832cf88..0fe4d65e9a3 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -259,7 +259,6 @@ func (sf *staticFrame) ProtobufFromInput(input []Input) *pb.JointPositions { // Geometries returns an object representing the 3D space associeted with the staticFrame. func (sf *staticFrame) Geometries(input []Input) (*GeometriesInFrame, error) { - fmt.Println("static frame geometry: ", sf.geometry) if sf.geometry == nil { return NewGeometriesInFrame(sf.Name(), nil), nil } From 9b0405d037bc2f2c1f3b4a94595351a77673b49b Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 9 May 2024 16:03:11 -0400 Subject: [PATCH 039/126] remove un-needed changes --- motionplan/constraint.go | 1 + motionplan/solverFrame.go | 4 +--- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/motionplan/constraint.go b/motionplan/constraint.go index 0d11effd224..16a363ccb51 100644 --- a/motionplan/constraint.go +++ b/motionplan/constraint.go @@ -269,6 +269,7 @@ func createAllCollisionConstraints( } constraintMap[defaultObstacleConstraintDesc] = obstacleConstraint } + if len(staticRobotGeometries) > 0 { // create constraint to keep moving geometries from hitting other geometries on robot that are not moving robotConstraint, err := NewCollisionConstraint( diff --git a/motionplan/solverFrame.go b/motionplan/solverFrame.go index ed8dd8c5cb9..1c362a6df88 100644 --- a/motionplan/solverFrame.go +++ b/motionplan/solverFrame.go @@ -217,10 +217,8 @@ func (sf *solverFrame) Interpolate(from, to []frame.Input, by float64) ([]frame. fromSubset := from[posIdx:dof] toSubset := to[posIdx:dof] posIdx = dof - var interpSub []frame.Input - var err error - interpSub, err = currFrame.Interpolate(fromSubset, toSubset, by) + interpSub, err := currFrame.Interpolate(fromSubset, toSubset, by) if err != nil { return nil, err } From c1977313c0be41cb6af811c729e910bcbf83d9a8 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 10 May 2024 15:38:08 -0400 Subject: [PATCH 040/126] post merge + fixing checkPlanRelative functionality that was lost upon rebase --- components/base/kinematicbase/execution.go | 2 +- .../base/kinematicbase/fake_kinematics.go | 19 +- .../base/kinematicbase/ptgKinematics.go | 8 +- motionplan/check.go | 273 ++++++++++++++---- motionplan/tpSpaceRRT_test.go | 251 ++++++++-------- services/motion/builtin/move_on_map_test.go | 3 +- services/motion/builtin/move_request.go | 33 +-- 7 files changed, 385 insertions(+), 204 deletions(-) diff --git a/components/base/kinematicbase/execution.go b/components/base/kinematicbase/execution.go index d74e93c1487..797d3f33f90 100644 --- a/components/base/kinematicbase/execution.go +++ b/components/base/kinematicbase/execution.go @@ -326,7 +326,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.planningFrame.Transform(currentInputs) + trajPose, err := ptgk.Kinematics().Transform(currentInputs) if err != nil { return nil, err } diff --git a/components/base/kinematicbase/fake_kinematics.go b/components/base/kinematicbase/fake_kinematics.go index f5eb3b8148b..b5787e79d8b 100644 --- a/components/base/kinematicbase/fake_kinematics.go +++ b/components/base/kinematicbase/fake_kinematics.go @@ -5,6 +5,7 @@ package kinematicbase import ( "context" "errors" + "math" "sync" "time" @@ -201,7 +202,11 @@ func WrapWithFakePTGKinematics( // construct execution frame executionFrame, err := referenceframe.New2DMobileModelFrame( b.Name().ShortName()+"ExecutionFrame", - planningFrame.DoF()[:3], + []referenceframe.Limit{ + {Min: math.Inf(-1), Max: math.Inf(1)}, + {Min: math.Inf(-1), Max: math.Inf(1)}, + {Min: -360, Max: 360}, + }, nil, ) if err != nil { @@ -217,8 +222,8 @@ func WrapWithFakePTGKinematics( return nil, errors.New("unable to cast ptgk frame to a PTG Provider") } ptgs := ptgProv.PTGSolvers() - traj := motionplan.Trajectory{{frame.Name(): zeroInput}} - path := motionplan.Path{{frame.Name(): referenceframe.NewPoseInFrame(origin.Parent(), spatialmath.Compose(origin.Pose(), sensorNoise))}} + traj := motionplan.Trajectory{{planningFrame.Name(): zeroInput}} + path := motionplan.Path{{planningFrame.Name(): referenceframe.NewPoseInFrame(origin.Parent(), spatialmath.Compose(origin.Pose(), sensorNoise))}} zeroPlan := motionplan.NewSimplePlan(path, traj) fk := &fakePTGKinematics{ @@ -261,9 +266,9 @@ func (fk *fakePTGKinematics) GoToInputs(ctx context.Context, inputSteps ...[]ref fk.currentInput = zeroInput fk.currentIndex = 0 - traj := motionplan.Trajectory{{fk.frame.Name(): zeroInput}} + traj := motionplan.Trajectory{{fk.planningFrame.Name(): zeroInput}} path := motionplan.Path{ - {fk.frame.Name(): referenceframe.NewPoseInFrame(fk.origin.Parent(), spatialmath.Compose(fk.origin.Pose(), fk.sensorNoise))}, + {fk.planningFrame.Name(): referenceframe.NewPoseInFrame(fk.origin.Parent(), spatialmath.Compose(fk.origin.Pose(), fk.sensorNoise))}, } fk.plan = motionplan.NewSimplePlan(path, traj) fk.inputLock.Unlock() @@ -345,8 +350,8 @@ func (fk *fakePTGKinematics) ExecutionState(ctx context.Context) (motionplan.Exe return motionplan.NewExecutionState( fk.plan, fk.currentIndex, - map[string][]referenceframe.Input{fk.frame.Name(): fk.currentInput}, - map[string]*referenceframe.PoseInFrame{fk.frame.Name(): pos}, + map[string][]referenceframe.Input{fk.Kinematics().Name(): fk.currentInput}, + map[string]*referenceframe.PoseInFrame{fk.ExecutionFrame().Name(): pos}, ) } diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index 79dd1374879..035c98ddf56 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -187,7 +187,11 @@ func (ptgk *ptgBaseKinematics) ExecutionFrame() referenceframe.Frame { func (ptgk *ptgBaseKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { ptgk.inputLock.RLock() defer ptgk.inputLock.RUnlock() - return ptgk.currentState.currentInputs, nil + + // ptgk.Localizer. + planningFrameInputs := ptgk.currentState.currentInputs + planningFrameInputs = append(planningFrameInputs, []referenceframe.Input{}...) + return planningFrameInputs, nil } func (ptgk *ptgBaseKinematics) ExecutionState(ctx context.Context) (motionplan.ExecutionState, error) { @@ -210,7 +214,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.ExecutionFrame().Name(): actualPIF}, ) } diff --git a/motionplan/check.go b/motionplan/check.go index 09665068070..4e4177c82af 100644 --- a/motionplan/check.go +++ b/motionplan/check.go @@ -11,6 +11,7 @@ import ( "go.viam.com/rdk/logging" "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/referenceframe" + frame "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -66,18 +67,23 @@ func checkPlanRelative( lookAheadDistanceMM float64, sfPlanner *planManager, ) error { + fmt.Println("executionState.currentInputs: ", executionState.currentInputs) + fmt.Println("executionState.currentPose: ", executionState.currentPose) + fmt.Println(" ") plan := executionState.Plan() - startingInputs := plan.Trajectory()[0] + wayPointIdx := executionState.Index() + startingInputs := plan.Trajectory()[wayPointIdx] + currentInputs := executionState.CurrentInputs() currentPoses := executionState.CurrentPoses() if currentPoses == nil { return errors.New("executionState had nil return from CurrentPoses") } - currentPoseIF, ok := currentPoses[checkFrame.Name()] + currentPoseIF, ok := currentPoses[checkFrame.Name()+"ExecutionFrame"] if !ok { - return errors.New("checkFrame not found in current pose map") + return errors.New("checkFrameExecutionFrame 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 @@ -130,29 +136,37 @@ func checkPlanRelative( return poseInWorld, nil } - // Check out path pose is valid + // 1. assume we are in the middle of executing an arc --> this is segments[0] + // 2. add segments for remaining parts of plan which have not yet been traveled + + // Check that path pose is valid stepEndPiF, ok := plan.Path()[wayPointIdx][checkFrame.Name()] if !ok { return errors.New("check frame given not in plan Path map") } + fmt.Println("stepEndPiF: ", spatialmath.PoseToProtobuf(stepEndPiF.Pose())) expectedArcEndInWorld, err := toWorld(stepEndPiF, plan.Trajectory()[wayPointIdx]) if err != nil { return err } + fmt.Println("expectedArcEndInWorld: ", spatialmath.PoseToProtobuf(expectedArcEndInWorld.Pose())) currentPoseInWorld, err := toWorld(currentPoseIF, currentInputs) if err != nil { return err } + fmt.Println("currentPoseInWorld: ", spatialmath.PoseToProtobuf(currentPoseInWorld.Pose())) arcInputs, ok := plan.Trajectory()[wayPointIdx][checkFrame.Name()] if !ok { return errors.New("given checkFrame had no inputs in trajectory map at current index") } + fmt.Println("arcInputs: ", arcInputs) fullArcPose, err := checkFrame.Transform(arcInputs) if err != nil { return err } + fmt.Println("fullArcPose: ", spatialmath.PoseToProtobuf(fullArcPose)) // 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. @@ -160,6 +174,8 @@ func checkPlanRelative( if !ok { return errors.New("given checkFrame had no inputs in CurrentInputs map") } + fmt.Println("frameCurrentInputs: ", frameCurrentInputs) + // here we need to add ExecutionFrame Inputs poseThroughArc, err := checkFrame.Transform(frameCurrentInputs) if err != nil { return err @@ -168,7 +184,7 @@ func checkPlanRelative( expectedCurrentPose := spatialmath.PoseBetweenInverse(remainingArcPose, expectedArcEndInWorld.Pose()) errorState := spatialmath.PoseBetween(expectedCurrentPose, currentPoseInWorld.Pose()) - planStartPiF, ok := plan.Path()[0][checkFrame.Name()] + planStartPiF, ok := plan.Path()[wayPointIdx][checkFrame.Name()] if !ok { return errors.New("check frame given not in plan Path map") } @@ -198,6 +214,7 @@ func checkPlanRelative( } // create a list of segments to iterate through + // FOCUS ON THIS NOW 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 @@ -205,7 +222,20 @@ func checkPlanRelative( if err != nil { return err } - arcEndInputs, err := sf.mapToSlice(plan.Trajectory()[wayPointIdx]) + poses, err := plan.Path().GetFramePoses(checkFrame.Name()) + if err != nil { + return err + } + currentWayPointTraj := plan.Trajectory()[wayPointIdx] + currentWayPointTraj[checkFrame.Name()+"ExecutionFrame"] = referenceframe.FloatsToInputs([]float64{ + poses[wayPointIdx].Point().X, + poses[wayPointIdx].Point().Y, + poses[wayPointIdx].Orientation().OrientationVectorRadians().Theta, + }) + // fmt.Println("checkFrameCurrentInputs: ", checkFrameCurrentInputs) + // fmt.Println("currentWayPointTraj: ", currentWayPointTraj) + // fmt.Println("expectedArcEndInWorld.Pose(): ", spatialmath.PoseToProtobuf(expectedArcEndInWorld.Pose())) + arcEndInputs, err := sf.mapToSlice(currentWayPointTraj) if err != nil { return err } @@ -219,6 +249,7 @@ func checkPlanRelative( EndConfiguration: arcEndInputs, Frame: sf, }) + fmt.Println("segments[0]: ", segments[0].String()) lastArcEndPose := currentArcEndPose // iterate through remaining plan and append remaining segments to check @@ -233,6 +264,7 @@ func checkPlanRelative( } thisArcEndPose := spatialmath.Compose(thisArcEndPoseInWorld.Pose(), errorState) // Starting inputs for relative frames should be all-zero + fmt.Println("plan.Trajectory()[i]: ", plan.Trajectory()[i]) startInputs := map[string][]referenceframe.Input{} for k, v := range plan.Trajectory()[i] { if k == checkFrame.Name() { @@ -241,6 +273,7 @@ func checkPlanRelative( startInputs[k] = v } } + fmt.Println("startInputs: ", startInputs) segment, err := createSegment(sf, lastArcEndPose, thisArcEndPose, startInputs, plan.Trajectory()[i]) if err != nil { return err @@ -248,7 +281,14 @@ func checkPlanRelative( lastArcEndPose = thisArcEndPose segments = append(segments, segment) } - return checkSegments(sfPlanner, segments, true, lookAheadDistanceMM) + fmt.Println(" ") + fmt.Println(" ") + fmt.Println(" ") + for _, s := range segments { + fmt.Println(s.String()) + fmt.Println(" ") + } + return checkSegments(sfPlanner, segments, lookAheadDistanceMM) } func checkPlanAbsolute( @@ -315,6 +355,40 @@ func checkPlanAbsolute( segments = append(segments, segment) } + return checkSegments(sfPlanner, segments, lookAheadDistanceMM) +} + +// createSegment is a function to ease segment creation for solver frames. +func createSegment( + sf *solverFrame, + currPose, nextPose spatialmath.Pose, + currInput, nextInput map[string][]referenceframe.Input, +) (*ik.Segment, error) { + var currInputSlice, nextInputSlice []referenceframe.Input + var err error + if currInput != nil { + currInputSlice, err = sf.mapToSlice(currInput) + if err != nil { + return nil, err + } + } + nextInputSlice, err = sf.mapToSlice(nextInput) + if err != nil { + return nil, err + } + + segment := &ik.Segment{ + StartPosition: currPose, + EndPosition: nextPose, + StartConfiguration: currInputSlice, + EndConfiguration: nextInputSlice, + Frame: sf, + } + + return segment, nil +} + +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. @@ -325,7 +399,7 @@ func checkPlanAbsolute( return err } for _, interpConfig := range interpolatedConfigurations { - poseInPath, err := sf.Transform(interpConfig) + poseInPath, err := sfPlanner.frame.Transform(interpConfig) if err != nil { return err } @@ -340,13 +414,17 @@ func checkPlanAbsolute( // 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} + 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. if isValid, err := sfPlanner.planOpts.CheckStateConstraints(interpolatedState); !isValid { - return fmt.Errorf("found error between positions %v and %v: %s", + 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(), err, ) } @@ -355,41 +433,142 @@ func checkPlanAbsolute( // Update total traveled distance after segment has been checked totalTravelDistanceMM += segment.EndPosition.Point().Distance(segment.StartPosition.Point()) } - - return checkSegments(sfPlanner, segments, false, lookAheadDistanceMM) + return nil } -// createSegment is a function to ease segment creation for solver frames. -func createSegment( - sf *solverFrame, - currPose, nextPose spatialmath.Pose, - currInput, nextInput map[string][]referenceframe.Input, -) (*ik.Segment, error) { - var currInputSlice, nextInputSlice []referenceframe.Input - var err error - if currInput != nil { - currInputSlice, err = sf.mapToSlice(currInput) - if err != nil { - return nil, err - } +func CheckPlanYoSelf( + checkFrame referenceframe.Frame, + plan Plan, + wayPointIdx int, + worldState *referenceframe.WorldState, + fs referenceframe.FrameSystem, + currentPose spatialmath.Pose, + currentInputs map[string][]referenceframe.Input, + errorState spatialmath.Pose, + lookAheadDistanceMM float64, + logger logging.Logger, +) error { + // ensure that we can actually perform the check + if len(plan.Path()) < 1 { + return errors.New("plan must have at least one element") } - nextInputSlice, err = sf.mapToSlice(nextInput) + + logger.Debugf("CheckPlan inputs: \n currentPosition: %v\n currentInputs: %v\n errorState: %v\n worldstate: %s", + spatialmath.PoseToProtobuf(currentPose), + currentInputs, + spatialmath.PoseToProtobuf(errorState), + worldState.String(), + ) + + // construct solverFrame + // Note that this requires all frames which move as part of the plan, to have an + // entry in the very first plan waypoint + sf, err := newSolverFrame(fs, checkFrame.Name(), frame.World, currentInputs) if err != nil { - return nil, err + return err } - segment := &ik.Segment{ - StartPosition: currPose, - EndPosition: nextPose, - StartConfiguration: currInputSlice, - EndConfiguration: nextInputSlice, - Frame: sf, + // construct planager + sfPlanner, err := newPlanManager(sf, logger, defaultRandomSeed) + if err != nil { + return err } - return segment, nil -} + // 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. + // The solver frame will have had its PTGs filled in the newPlanManager() call, if applicable. + relative := len(sf.PTGSolvers()) > 0 + + // offset the plan using the errorState + offsetPlan := OffsetPlan(plan, errorState) + + // get plan poses for checkFrame + poses, err := offsetPlan.Path().GetFramePoses(checkFrame.Name()) + if err != nil { + return err + } + + // setup the planOpts + if sfPlanner.planOpts, err = sfPlanner.plannerSetupFromMoveRequest( + currentPose, + poses[len(poses)-1], + currentInputs, + worldState, + nil, // no pb.Constraints + nil, // no plannOpts + ); err != nil { + return err + } + + // create a list of segments to iterate through + segments := make([]*ik.Segment, 0, len(poses)-wayPointIdx) + if relative { + // 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 + } + + // pre-pend to segments so we can connect to the input we have not finished actuating yet + segments = append(segments, &ik.Segment{ + StartPosition: currentPose, + EndPosition: poses[wayPointIdx], + StartConfiguration: checkFrameCurrentInputs, + EndConfiguration: checkFrameCurrentInputs, + Frame: sf, + }) + } + + // function to ease further segment creation + createSegment := func( + currPose, nextPose spatialmath.Pose, + currInput, nextInput map[string][]frame.Input, + ) (*ik.Segment, error) { + currInputSlice, err := sf.mapToSlice(currInput) + if err != nil { + return nil, err + } + nextInputSlice, err := sf.mapToSlice(nextInput) + if err != nil { + return nil, err + } + // If we are working with a PTG plan we redefine the startConfiguration in terms of the endConfiguration. + // This allows us the properly interpolate along the same arc family and sub-arc within that family. + if relative { + currInputSlice = nextInputSlice + } + return &ik.Segment{ + StartPosition: currPose, + EndPosition: nextPose, + StartConfiguration: currInputSlice, + EndConfiguration: nextInputSlice, + Frame: sf, + }, nil + } + + // iterate through remaining plan and append remaining segments to check + for i := wayPointIdx; i < len(offsetPlan.Path())-1; i++ { + currInput := offsetPlan.Trajectory()[i] + nextInput := offsetPlan.Trajectory()[i+1] + if relative { + currInput[checkFrame.Name()+"ExecutionFrame"] = frame.FloatsToInputs( + []float64{poses[i].Point().X, poses[i].Point().Y, poses[i].Orientation().OrientationVectorRadians().Theta}, + ) + nextInput[checkFrame.Name()+"ExecutionFrame"] = frame.FloatsToInputs( + []float64{poses[i].Point().X, poses[i].Point().Y, poses[i].Orientation().OrientationVectorRadians().Theta}, + ) + if i == 0 { + currInput = nextInput + } + } + segment, err := createSegment(poses[i], poses[i+1], currInput, nextInput) + if err != nil { + return err + } + segments = append(segments, segment) + } -func checkSegments(sfPlanner *planManager, segments []*ik.Segment, relative bool, 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. @@ -400,7 +579,7 @@ func checkSegments(sfPlanner *planManager, segments []*ik.Segment, relative bool return err } for _, interpConfig := range interpolatedConfigurations { - poseInPath, err := sfPlanner.frame.Transform(interpConfig) + poseInPath, err := sf.Transform(interpConfig) if err != nil { return err } @@ -411,23 +590,16 @@ 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: sf} + interpolatedState.Configuration = interpConfig // 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 constraint violation or collision in segment between %v and %v at %v: %s", + return fmt.Errorf("found error between positions %v and %v: %s", segment.StartPosition.Point(), segment.EndPosition.Point(), - interpolatedState.Position.Point(), err, ) } @@ -436,5 +608,6 @@ func checkSegments(sfPlanner *planManager, segments []*ik.Segment, relative bool // Update total traveled distance after segment has been checked totalTravelDistanceMM += segment.EndPosition.Point().Distance(segment.StartPosition.Point()) } + return nil } diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index f1e1c445a37..0aeaaa62e77 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -4,6 +4,7 @@ package motionplan import ( "context" + "fmt" "math" "math/rand" "testing" @@ -282,7 +283,7 @@ func TestPtgCheckPlan(t *testing.T) { plan, err := newRRTPlan(nodes, sf, true) test.That(t, err, test.ShouldBeNil) - startPose := spatialmath.NewPoseFromPoint(r3.Vector{0, 0, 0}) + // startPose := spatialmath.NewPoseFromPoint(r3.Vector{0, 0, 0}) inputs := plan.Trajectory()[0] // NOTE: WE NEED TO ADD AN EXECUTION FRAME TO THE CHECKING FRAMESYSTEM SINCE WE ONLY WANT TO RELY ON USING INPUTS @@ -301,42 +302,43 @@ func TestPtgCheckPlan(t *testing.T) { err = tfFrameSystem.MergeFrameSystem(fs, executionFrame) test.That(t, err, test.ShouldBeNil) - inputs[executionFrame.Name()] = referenceframe.FloatsToInputs(make([]float64, 3)) - - 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) - }) + inputs[executionFrame.Name()] = referenceframe.FloatsToInputs(make([]float64, len(executionFrame.DoF()))) + + // 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{ + // executionFrame.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{ + // executionFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), + // }, + // } + // err = CheckPlan(ackermanFrame, executionState, worldState, fs, 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})) @@ -358,6 +360,7 @@ func TestPtgCheckPlan(t *testing.T) { test.That(t, err, test.ShouldBeNil) err = tfFrameSystem.AddFrame(cameraFrame, cameraOriginFrame) test.That(t, err, test.ShouldBeNil) + inputs[cameraFrame.Name()] = referenceframe.FloatsToInputs(make([]float64, len(cameraFrame.DoF()))) t.Run("obstacles NOT in world frame - no collision - integration test", func(t *testing.T) { obstacle, err := spatialmath.NewBox( @@ -375,95 +378,101 @@ func TestPtgCheckPlan(t *testing.T) { index: 1, currentInputs: inputs, currentPose: map[string]*referenceframe.PoseInFrame{ - ackermanFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), + executionFrame.Name(): plan.Path()[1][ackermanFrame.Name()], }, } - err = CheckPlan(ackermanFrame, executionState, worldState, fs, math.Inf(1), logger) + fmt.Println(plan.Path()) + fmt.Println(plan.Trajectory()) + fmt.Println(" ") + err = CheckPlan(ackermanFrame, executionState, worldState, tfFrameSystem, 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) - }) + // 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{ + // executionFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), + // }, + // } + // err = CheckPlan(ackermanFrame, executionState, worldState, tfFrameSystem, 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) + // }) + + // 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{ + // executionFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), + // }, + // } + // err = CheckPlan(ackermanFrame, executionState, worldState, tfFrameSystem, math.Inf(1), logger) + // test.That(t, err, test.ShouldBeNil) + // test.That(t, strings.Contains(err.Error(), "found constraint violation or collision in segment between"), test.ShouldBeTrue) + // }) + + // 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{ + // executionFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), + // }, + // } + // err = CheckPlan(ackermanFrame, executionState, worldState, tfFrameSystem, math.Inf(1), logger) + // test.That(t, err, test.ShouldBeNil) + // test.That(t, strings.Contains(err.Error(), "found constraint violation or collision in segment between"), test.ShouldBeTrue) + // }) } func planToTpspaceRec(plan Plan, f referenceframe.Frame) ([]node, error) { diff --git a/services/motion/builtin/move_on_map_test.go b/services/motion/builtin/move_on_map_test.go index 4c475a37bad..a025c052d9c 100644 --- a/services/motion/builtin/move_on_map_test.go +++ b/services/motion/builtin/move_on_map_test.go @@ -603,7 +603,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( @@ -618,7 +617,7 @@ func TestMoveOnMapStaticObs(t *testing.T) { 1, referenceframe.StartPositions(mr.planRequest.FrameSystem), map[string]*referenceframe.PoseInFrame{ - mr.planRequest.Frame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewPose( + mr.kinematicBase.ExecutionFrame().Name(): referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewPose( r3.Vector{X: 0.58772e3, Y: -0.80826e3, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 0}, )), diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index f3bbcc7e267..f2e2ec24975 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -180,10 +180,17 @@ func (mr *moveRequest) getTransientDetections( camName.ShortName(), ) - currentInputs, _, err := mr.getCurrentInputsAndPosition(ctx) + baseExecutionState, err := mr.kinematicBase.ExecutionState(ctx) if err != nil { return nil, err } + inputMap := referenceframe.StartPositions(mr.absoluteFS) + inputMap[mr.kinematicBase.Name().ShortName()] = baseExecutionState.CurrentInputs()[mr.kinematicBase.Name().ShortName()] + inputMap[mr.kinematicBase.ExecutionFrame().Name()] = referenceframe.FloatsToInputs([]float64{ + baseExecutionState.CurrentPoses()[mr.kinematicBase.ExecutionFrame().Name()].Pose().Point().X, + baseExecutionState.CurrentPoses()[mr.kinematicBase.ExecutionFrame().Name()].Pose().Point().Y, + baseExecutionState.CurrentPoses()[mr.kinematicBase.ExecutionFrame().Name()].Pose().Point().Z, + }) detections, err := visSrvc.GetObjectPointClouds(ctx, camName.Name, nil) if err != nil { @@ -200,12 +207,9 @@ func (mr *moveRequest) getTransientDetections( label += "_" + geometry.Label() } geometry.SetLabel(label) - tf, err := mr.absoluteFS.Transform( - currentInputs, - referenceframe.NewGeometriesInFrame( - camName.ShortName(), []spatialmath.Geometry{geometry}, - ), + inputMap, + referenceframe.NewGeometriesInFrame(camName.ShortName(), []spatialmath.Geometry{geometry}), referenceframe.World, ) if err != nil { @@ -270,6 +274,7 @@ func (mr *moveRequest) obstaclesIntersectPlan( // configuration rather than the zero inputs inputMap := referenceframe.StartPositions(mr.planRequest.FrameSystem) inputMap[mr.kinematicBase.Name().ShortName()] = baseExecutionState.CurrentInputs()[mr.kinematicBase.Name().ShortName()] + inputMap[mr.kinematicBase.ExecutionFrame().Name()] = baseExecutionState.CurrentInputs()[mr.kinematicBase.ExecutionFrame().Name()] executionState, err := motionplan.NewExecutionState( baseExecutionState.Plan(), baseExecutionState.Index(), @@ -281,7 +286,7 @@ func (mr *moveRequest) obstaclesIntersectPlan( } mr.logger.CDebugf(ctx, "CheckPlan inputs: \n currentPosition: %v\n currentInputs: %v\n worldstate: %s", - spatialmath.PoseToProtobuf(executionState.CurrentPoses()[mr.kinematicBase.Name().ShortName()].Pose()), + spatialmath.PoseToProtobuf(executionState.CurrentPoses()[mr.kinematicBase.ExecutionFrame().Name()].Pose()), inputMap, worldState.String(), ) @@ -302,20 +307,6 @@ func (mr *moveRequest) obstaclesIntersectPlan( return state.ExecuteResponse{}, nil } -func (mr *moveRequest) getCurrentInputsAndPosition( - ctx context.Context, -) (map[string][]referenceframe.Input, *referenceframe.PoseInFrame, error) { - currentInputs := referenceframe.StartPositions(mr.absoluteFS) - pif, err := mr.kinematicBase.CurrentPosition(ctx) - if err != nil { - return nil, nil, err - } - currentInputs[mr.kinematicBase.Name().ShortName()+"ExecutionFrame"] = referenceframe.FloatsToInputs( - []float64{pif.Pose().Point().X, pif.Pose().Point().Y, pif.Pose().Orientation().OrientationVectorRadians().Theta}, - ) - return currentInputs, pif, nil -} - func kbOptionsFromCfg(motionCfg *validatedMotionConfiguration, validatedExtra validatedExtra) kinematicbase.Options { kinematicsOptions := kinematicbase.NewKinematicBaseOptions() From 01b7d935b2ba262a76de980021926e4b54233723 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 13 May 2024 11:27:47 -0400 Subject: [PATCH 041/126] fixed TestPtgCheckPlan and accompanying relative plan checking code --- motionplan/check.go | 254 ++++------------------------------ motionplan/tpSpaceRRT_test.go | 247 ++++++++++++++++----------------- 2 files changed, 150 insertions(+), 351 deletions(-) diff --git a/motionplan/check.go b/motionplan/check.go index 4e4177c82af..2e73516cd5e 100644 --- a/motionplan/check.go +++ b/motionplan/check.go @@ -67,25 +67,6 @@ func checkPlanRelative( lookAheadDistanceMM float64, sfPlanner *planManager, ) error { - fmt.Println("executionState.currentInputs: ", executionState.currentInputs) - fmt.Println("executionState.currentPose: ", executionState.currentPose) - fmt.Println(" ") - plan := executionState.Plan() - wayPointIdx := executionState.Index() - startingInputs := plan.Trajectory()[wayPointIdx] - - currentInputs := executionState.CurrentInputs() - currentPoses := executionState.CurrentPoses() - if currentPoses == nil { - return errors.New("executionState had nil return from CurrentPoses") - } - currentPoseIF, ok := currentPoses[checkFrame.Name()+"ExecutionFrame"] - if !ok { - return errors.New("checkFrameExecutionFrame not found in current pose map") - } - - 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 @@ -136,37 +117,46 @@ func checkPlanRelative( return poseInWorld, nil } - // 1. assume we are in the middle of executing an arc --> this is segments[0] - // 2. add segments for remaining parts of plan which have not yet been traveled + plan := executionState.Plan() + wayPointIdx := executionState.Index() + startingInputs := executionState.currentInputs + + currentInputs := executionState.CurrentInputs() + currentPoses := executionState.CurrentPoses() + if currentPoses == nil { + return errors.New("executionState had nil return from CurrentPoses") + } + executionFrameName := checkFrame.Name() + "ExecutionFrame" + currentPoseIF, ok := currentPoses[executionFrameName] + if !ok { + return errors.New("checkFrameExecutionFrame not found in current pose map") + } + + sf := sfPlanner.frame // Check that path pose is valid stepEndPiF, ok := plan.Path()[wayPointIdx][checkFrame.Name()] if !ok { return errors.New("check frame given not in plan Path map") } - fmt.Println("stepEndPiF: ", spatialmath.PoseToProtobuf(stepEndPiF.Pose())) expectedArcEndInWorld, err := toWorld(stepEndPiF, plan.Trajectory()[wayPointIdx]) if err != nil { return err } - fmt.Println("expectedArcEndInWorld: ", spatialmath.PoseToProtobuf(expectedArcEndInWorld.Pose())) currentPoseInWorld, err := toWorld(currentPoseIF, currentInputs) if err != nil { return err } - fmt.Println("currentPoseInWorld: ", spatialmath.PoseToProtobuf(currentPoseInWorld.Pose())) arcInputs, ok := plan.Trajectory()[wayPointIdx][checkFrame.Name()] if !ok { return errors.New("given checkFrame had no inputs in trajectory map at current index") } - fmt.Println("arcInputs: ", arcInputs) fullArcPose, err := checkFrame.Transform(arcInputs) if err != nil { return err } - fmt.Println("fullArcPose: ", spatialmath.PoseToProtobuf(fullArcPose)) // 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. @@ -174,8 +164,6 @@ func checkPlanRelative( if !ok { return errors.New("given checkFrame had no inputs in CurrentInputs map") } - fmt.Println("frameCurrentInputs: ", frameCurrentInputs) - // here we need to add ExecutionFrame Inputs poseThroughArc, err := checkFrame.Transform(frameCurrentInputs) if err != nil { return err @@ -213,8 +201,6 @@ func checkPlanRelative( return err } - // create a list of segments to iterate through - // FOCUS ON THIS NOW 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 @@ -222,6 +208,7 @@ func checkPlanRelative( if err != nil { return err } + poses, err := plan.Path().GetFramePoses(checkFrame.Name()) if err != nil { return err @@ -232,15 +219,14 @@ func checkPlanRelative( poses[wayPointIdx].Point().Y, poses[wayPointIdx].Orientation().OrientationVectorRadians().Theta, }) - // fmt.Println("checkFrameCurrentInputs: ", checkFrameCurrentInputs) - // fmt.Println("currentWayPointTraj: ", currentWayPointTraj) - // fmt.Println("expectedArcEndInWorld.Pose(): ", spatialmath.PoseToProtobuf(expectedArcEndInWorld.Pose())) + arcEndInputs, err := sf.mapToSlice(currentWayPointTraj) if err != nil { return err } currentArcEndPose := spatialmath.Compose(expectedArcEndInWorld.Pose(), errorState) + // 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(), @@ -249,9 +235,9 @@ func checkPlanRelative( EndConfiguration: arcEndInputs, Frame: sf, }) - fmt.Println("segments[0]: ", segments[0].String()) 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()] @@ -264,7 +250,6 @@ func checkPlanRelative( } thisArcEndPose := spatialmath.Compose(thisArcEndPoseInWorld.Pose(), errorState) // Starting inputs for relative frames should be all-zero - fmt.Println("plan.Trajectory()[i]: ", plan.Trajectory()[i]) startInputs := map[string][]referenceframe.Input{} for k, v := range plan.Trajectory()[i] { if k == checkFrame.Name() { @@ -273,21 +258,19 @@ func checkPlanRelative( startInputs[k] = v } } - fmt.Println("startInputs: ", startInputs) - segment, err := createSegment(sf, lastArcEndPose, thisArcEndPose, startInputs, plan.Trajectory()[i]) + + startInputs[executionFrameName] = frame.FloatsToInputs( + []float64{lastArcEndPose.Point().X, lastArcEndPose.Point().Y, lastArcEndPose.Orientation().OrientationVectorRadians().Theta}, + ) + nextInputs := plan.Trajectory()[i] + nextInputs[executionFrameName] = startInputs[executionFrameName] + segment, err := createSegment(sf, lastArcEndPose, thisArcEndPose, startInputs, nextInputs) if err != nil { return err } lastArcEndPose = thisArcEndPose segments = append(segments, segment) } - fmt.Println(" ") - fmt.Println(" ") - fmt.Println(" ") - for _, s := range segments { - fmt.Println(s.String()) - fmt.Println(" ") - } return checkSegments(sfPlanner, segments, lookAheadDistanceMM) } @@ -410,10 +393,7 @@ func checkSegments(sfPlanner *planManager, segments []*ik.Segment, lookAheadDist 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. + // construct state representing where we currently are in the path interpolatedState := &ik.State{ Frame: sfPlanner.frame, Configuration: interpConfig, @@ -424,7 +404,7 @@ func checkSegments(sfPlanner *planManager, segments []*ik.Segment, lookAheadDist 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, ) } @@ -435,179 +415,3 @@ func checkSegments(sfPlanner *planManager, segments []*ik.Segment, lookAheadDist } return nil } - -func CheckPlanYoSelf( - checkFrame referenceframe.Frame, - plan Plan, - wayPointIdx int, - worldState *referenceframe.WorldState, - fs referenceframe.FrameSystem, - currentPose spatialmath.Pose, - currentInputs map[string][]referenceframe.Input, - errorState spatialmath.Pose, - lookAheadDistanceMM float64, - logger logging.Logger, -) error { - // ensure that we can actually perform the check - if len(plan.Path()) < 1 { - return errors.New("plan must have at least one element") - } - - logger.Debugf("CheckPlan inputs: \n currentPosition: %v\n currentInputs: %v\n errorState: %v\n worldstate: %s", - spatialmath.PoseToProtobuf(currentPose), - currentInputs, - spatialmath.PoseToProtobuf(errorState), - worldState.String(), - ) - - // construct solverFrame - // Note that this requires all frames which move as part of the plan, to have an - // entry in the very first plan waypoint - sf, err := newSolverFrame(fs, checkFrame.Name(), frame.World, currentInputs) - if err != nil { - return err - } - - // construct planager - sfPlanner, err := newPlanManager(sf, logger, defaultRandomSeed) - 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. - // The solver frame will have had its PTGs filled in the newPlanManager() call, if applicable. - relative := len(sf.PTGSolvers()) > 0 - - // offset the plan using the errorState - offsetPlan := OffsetPlan(plan, errorState) - - // get plan poses for checkFrame - poses, err := offsetPlan.Path().GetFramePoses(checkFrame.Name()) - if err != nil { - return err - } - - // setup the planOpts - if sfPlanner.planOpts, err = sfPlanner.plannerSetupFromMoveRequest( - currentPose, - poses[len(poses)-1], - currentInputs, - worldState, - nil, // no pb.Constraints - nil, // no plannOpts - ); err != nil { - return err - } - - // create a list of segments to iterate through - segments := make([]*ik.Segment, 0, len(poses)-wayPointIdx) - if relative { - // 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 - } - - // pre-pend to segments so we can connect to the input we have not finished actuating yet - segments = append(segments, &ik.Segment{ - StartPosition: currentPose, - EndPosition: poses[wayPointIdx], - StartConfiguration: checkFrameCurrentInputs, - EndConfiguration: checkFrameCurrentInputs, - Frame: sf, - }) - } - - // function to ease further segment creation - createSegment := func( - currPose, nextPose spatialmath.Pose, - currInput, nextInput map[string][]frame.Input, - ) (*ik.Segment, error) { - currInputSlice, err := sf.mapToSlice(currInput) - if err != nil { - return nil, err - } - nextInputSlice, err := sf.mapToSlice(nextInput) - if err != nil { - return nil, err - } - // If we are working with a PTG plan we redefine the startConfiguration in terms of the endConfiguration. - // This allows us the properly interpolate along the same arc family and sub-arc within that family. - if relative { - currInputSlice = nextInputSlice - } - return &ik.Segment{ - StartPosition: currPose, - EndPosition: nextPose, - StartConfiguration: currInputSlice, - EndConfiguration: nextInputSlice, - Frame: sf, - }, nil - } - - // iterate through remaining plan and append remaining segments to check - for i := wayPointIdx; i < len(offsetPlan.Path())-1; i++ { - currInput := offsetPlan.Trajectory()[i] - nextInput := offsetPlan.Trajectory()[i+1] - if relative { - currInput[checkFrame.Name()+"ExecutionFrame"] = frame.FloatsToInputs( - []float64{poses[i].Point().X, poses[i].Point().Y, poses[i].Orientation().OrientationVectorRadians().Theta}, - ) - nextInput[checkFrame.Name()+"ExecutionFrame"] = frame.FloatsToInputs( - []float64{poses[i].Point().X, poses[i].Point().Y, poses[i].Orientation().OrientationVectorRadians().Theta}, - ) - if i == 0 { - currInput = nextInput - } - } - segment, err := createSegment(poses[i], poses[i+1], currInput, nextInput) - if err != nil { - return err - } - 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 - } - - // 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: sf} - interpolatedState.Configuration = interpConfig - - // 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 nil -} diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index 0aeaaa62e77..d42413ec46d 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -4,9 +4,9 @@ package motionplan import ( "context" - "fmt" "math" "math/rand" + "strings" "testing" "github.com/golang/geo/r3" @@ -283,7 +283,7 @@ func TestPtgCheckPlan(t *testing.T) { plan, err := newRRTPlan(nodes, sf, true) test.That(t, err, test.ShouldBeNil) - // startPose := spatialmath.NewPoseFromPoint(r3.Vector{0, 0, 0}) + startPose := spatialmath.NewZeroPose() inputs := plan.Trajectory()[0] // NOTE: WE NEED TO ADD AN EXECUTION FRAME TO THE CHECKING FRAMESYSTEM SINCE WE ONLY WANT TO RELY ON USING INPUTS @@ -304,41 +304,41 @@ func TestPtgCheckPlan(t *testing.T) { test.That(t, err, test.ShouldBeNil) inputs[executionFrame.Name()] = referenceframe.FloatsToInputs(make([]float64, len(executionFrame.DoF()))) - // 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{ - // executionFrame.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{ - // executionFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), - // }, - // } - // err = CheckPlan(ackermanFrame, executionState, worldState, fs, 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) - // }) + 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{ + executionFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), + }, + } + err = CheckPlan(ackermanFrame, executionState, nil, tfFrameSystem, 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{ + executionFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), + }, + } + err = CheckPlan(ackermanFrame, executionState, worldState, tfFrameSystem, 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})) @@ -376,103 +376,98 @@ func TestPtgCheckPlan(t *testing.T) { executionState := ExecutionState{ plan: plan, index: 1, - currentInputs: inputs, + currentInputs: inputs, // zero'd inputs are incorrect here currentPose: map[string]*referenceframe.PoseInFrame{ executionFrame.Name(): plan.Path()[1][ackermanFrame.Name()], }, } - fmt.Println(plan.Path()) - fmt.Println(plan.Trajectory()) - fmt.Println(" ") err = CheckPlan(ackermanFrame, executionState, worldState, tfFrameSystem, 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{ - // executionFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), - // }, - // } - // err = CheckPlan(ackermanFrame, executionState, worldState, tfFrameSystem, 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) - // }) - - // 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{ - // executionFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), - // }, - // } - // err = CheckPlan(ackermanFrame, executionState, worldState, tfFrameSystem, math.Inf(1), logger) - // test.That(t, err, test.ShouldBeNil) - // test.That(t, strings.Contains(err.Error(), "found constraint violation or collision in segment between"), test.ShouldBeTrue) - // }) - - // 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{ - // executionFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), - // }, - // } - // err = CheckPlan(ackermanFrame, executionState, worldState, tfFrameSystem, math.Inf(1), logger) - // test.That(t, err, test.ShouldBeNil) - // test.That(t, strings.Contains(err.Error(), "found constraint violation or collision in segment between"), test.ShouldBeTrue) - // }) + 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{ + executionFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), + }, + } + err = CheckPlan(ackermanFrame, executionState, worldState, tfFrameSystem, 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) + }) + + 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{ + executionFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), + }, + } + err = CheckPlan(ackermanFrame, executionState, worldState, tfFrameSystem, 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{ + executionFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), + }, + } + err = CheckPlan(ackermanFrame, executionState, worldState, tfFrameSystem, math.Inf(1), logger) + test.That(t, err, test.ShouldBeNil) + }) } func planToTpspaceRec(plan Plan, f referenceframe.Frame) ([]node, error) { From eaf8ffce47d88e2a5e0780f88db78d6d7cad7ee0 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 13 May 2024 12:42:27 -0400 Subject: [PATCH 042/126] fix test TestMoveOnMapStaticObs + move request set up nits for feeding into CheckPlan --- services/motion/builtin/move_on_map_test.go | 8 ++++++-- services/motion/builtin/move_request.go | 6 +++++- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/services/motion/builtin/move_on_map_test.go b/services/motion/builtin/move_on_map_test.go index a025c052d9c..a39f4cd92e3 100644 --- a/services/motion/builtin/move_on_map_test.go +++ b/services/motion/builtin/move_on_map_test.go @@ -612,10 +612,14 @@ func TestMoveOnMapStaticObs(t *testing.T) { }, nil, ) test.That(t, err, test.ShouldBeNil) + currentInputs := referenceframe.StartPositions(mr.absoluteFS) + currentInputs[mr.kinematicBase.ExecutionFrame().Name()] = referenceframe.FloatsToInputs( + []float64{0.58772e3, -0.80826e3, 0}, + ) executionState, err := motionplan.NewExecutionState( plan, 1, - referenceframe.StartPositions(mr.planRequest.FrameSystem), + currentInputs, map[string]*referenceframe.PoseInFrame{ mr.kinematicBase.ExecutionFrame().Name(): referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewPose( r3.Vector{X: 0.58772e3, Y: -0.80826e3, Z: 0}, @@ -628,7 +632,7 @@ func TestMoveOnMapStaticObs(t *testing.T) { mr.planRequest.Frame, executionState, wrldSt, - mr.planRequest.FrameSystem, + mr.absoluteFS, lookAheadDistanceMM, logger, ) diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index f2e2ec24975..b8bc4bbbf36 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -274,7 +274,11 @@ func (mr *moveRequest) obstaclesIntersectPlan( // configuration rather than the zero inputs inputMap := referenceframe.StartPositions(mr.planRequest.FrameSystem) inputMap[mr.kinematicBase.Name().ShortName()] = baseExecutionState.CurrentInputs()[mr.kinematicBase.Name().ShortName()] - inputMap[mr.kinematicBase.ExecutionFrame().Name()] = baseExecutionState.CurrentInputs()[mr.kinematicBase.ExecutionFrame().Name()] + inputMap[mr.kinematicBase.ExecutionFrame().Name()] = referenceframe.FloatsToInputs([]float64{ + baseExecutionState.CurrentPoses()[mr.kinematicBase.ExecutionFrame().Name()].Pose().Point().X, + baseExecutionState.CurrentPoses()[mr.kinematicBase.ExecutionFrame().Name()].Pose().Point().Y, + baseExecutionState.CurrentPoses()[mr.kinematicBase.ExecutionFrame().Name()].Pose().Point().Z, + }) executionState, err := motionplan.NewExecutionState( baseExecutionState.Plan(), baseExecutionState.Index(), From 7f7e4c695ed7b45de4003b0e3dc88b9054aa3f95 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 13 May 2024 12:43:21 -0400 Subject: [PATCH 043/126] move request nits + feed in proper framesystem for checking of plan --- services/motion/builtin/move_request.go | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index b8bc4bbbf36..e371b5d021e 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -272,7 +272,7 @@ 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 := referenceframe.StartPositions(mr.absoluteFS) inputMap[mr.kinematicBase.Name().ShortName()] = baseExecutionState.CurrentInputs()[mr.kinematicBase.Name().ShortName()] inputMap[mr.kinematicBase.ExecutionFrame().Name()] = referenceframe.FloatsToInputs([]float64{ baseExecutionState.CurrentPoses()[mr.kinematicBase.ExecutionFrame().Name()].Pose().Point().X, @@ -299,7 +299,7 @@ func (mr *moveRequest) obstaclesIntersectPlan( mr.kinematicBase.Kinematics(), // frame we wish to check for collisions executionState, worldState, // detected obstacles by this instance of camera + service - mr.planRequest.FrameSystem, + mr.absoluteFS, lookAheadDistanceMM, mr.planRequest.Logger, ); err != nil { From 02fe62f4ccd968dc1b413b951323fee4abcf858a Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 13 May 2024 12:48:46 -0400 Subject: [PATCH 044/126] changes from linter --- components/base/kinematicbase/fake_kinematics.go | 4 +++- motionplan/check.go | 3 +-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/components/base/kinematicbase/fake_kinematics.go b/components/base/kinematicbase/fake_kinematics.go index b5787e79d8b..408f04463b1 100644 --- a/components/base/kinematicbase/fake_kinematics.go +++ b/components/base/kinematicbase/fake_kinematics.go @@ -223,7 +223,9 @@ func WrapWithFakePTGKinematics( } ptgs := ptgProv.PTGSolvers() traj := motionplan.Trajectory{{planningFrame.Name(): zeroInput}} - path := motionplan.Path{{planningFrame.Name(): referenceframe.NewPoseInFrame(origin.Parent(), spatialmath.Compose(origin.Pose(), sensorNoise))}} + path := motionplan.Path{ + {planningFrame.Name(): referenceframe.NewPoseInFrame(origin.Parent(), spatialmath.Compose(origin.Pose(), sensorNoise))}, + } zeroPlan := motionplan.NewSimplePlan(path, traj) fk := &fakePTGKinematics{ diff --git a/motionplan/check.go b/motionplan/check.go index 2e73516cd5e..5fbdcba0a57 100644 --- a/motionplan/check.go +++ b/motionplan/check.go @@ -11,7 +11,6 @@ import ( "go.viam.com/rdk/logging" "go.viam.com/rdk/motionplan/ik" "go.viam.com/rdk/referenceframe" - frame "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" ) @@ -259,7 +258,7 @@ func checkPlanRelative( } } - startInputs[executionFrameName] = frame.FloatsToInputs( + startInputs[executionFrameName] = referenceframe.FloatsToInputs( []float64{lastArcEndPose.Point().X, lastArcEndPose.Point().Y, lastArcEndPose.Orientation().OrientationVectorRadians().Theta}, ) nextInputs := plan.Trajectory()[i] From c3569dd122b32002e02ac988a4a604216994f1d9 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 13 May 2024 13:53:49 -0400 Subject: [PATCH 045/126] nits --- motionplan/check.go | 3 ++- referenceframe/frame_system.go | 15 --------------- services/motion/builtin/move_on_map_test.go | 1 + 3 files changed, 3 insertions(+), 16 deletions(-) diff --git a/motionplan/check.go b/motionplan/check.go index 5fbdcba0a57..1b1dc678448 100644 --- a/motionplan/check.go +++ b/motionplan/check.go @@ -392,7 +392,8 @@ func checkSegments(sfPlanner *planManager, segments []*ik.Segment, lookAheadDist return nil } - // construct state representing where we currently are in the path + // 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, diff --git a/referenceframe/frame_system.go b/referenceframe/frame_system.go index b9950a9a871..5a06a5f0fe4 100644 --- a/referenceframe/frame_system.go +++ b/referenceframe/frame_system.go @@ -712,18 +712,3 @@ func poseFromPositions(frame Frame, positions map[string][]Input) (spatial.Pose, } return frame.Transform(inputs) } - -// AddFrameBetween takes an existing framesystem which has general form: -// originalFS := world - f1 - f2 - ... -// and places the middle frame between f1 and world resulting in -// newFS := world - middle - f1 - f2 ... -func AddFrameBetween(middle Frame, originalFS FrameSystem) (FrameSystem, error) { - newFS := NewEmptyFrameSystem("with-in-between Frame") - if err := newFS.AddFrame(middle, newFS.World()); err != nil { - return nil, err - } - if err := newFS.MergeFrameSystem(originalFS, middle); err != nil { - return nil, err - } - return newFS, nil -} diff --git a/services/motion/builtin/move_on_map_test.go b/services/motion/builtin/move_on_map_test.go index a39f4cd92e3..178e2268567 100644 --- a/services/motion/builtin/move_on_map_test.go +++ b/services/motion/builtin/move_on_map_test.go @@ -637,6 +637,7 @@ func TestMoveOnMapStaticObs(t *testing.T) { 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) { From cfa4aea13297afe226766c499cbed1ea91422410 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 17 May 2024 16:14:15 -0400 Subject: [PATCH 046/126] fix explore err string check --- services/motion/explore/explore.go | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/services/motion/explore/explore.go b/services/motion/explore/explore.go index e189a294b22..9029f791721 100644 --- a/services/motion/explore/explore.go +++ b/services/motion/explore/explore.go @@ -375,7 +375,7 @@ func (ms *explore) checkForObstacles( } errCounterCurrentInputs++ } - + fmt.Println("WE ARE ABOUT TO ENTER CHECKPLAN") // Check plan for transient obstacles err = motionplan.CheckPlan( kb.Kinematics(), @@ -385,8 +385,10 @@ func (ms *explore) checkForObstacles( lookAheadDistanceMM, ms.logger, ) + fmt.Println("HELLLL YEA WE MADE IT OUT OF THE FUNCTION") + fmt.Println("err: ", err) 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 From c6c995cd6c269b1230f5ef91b39975d6ec629751 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 17 May 2024 16:23:59 -0400 Subject: [PATCH 047/126] remove prints --- services/motion/explore/explore.go | 3 --- 1 file changed, 3 deletions(-) diff --git a/services/motion/explore/explore.go b/services/motion/explore/explore.go index 9029f791721..0ab178e1256 100644 --- a/services/motion/explore/explore.go +++ b/services/motion/explore/explore.go @@ -375,7 +375,6 @@ func (ms *explore) checkForObstacles( } errCounterCurrentInputs++ } - fmt.Println("WE ARE ABOUT TO ENTER CHECKPLAN") // Check plan for transient obstacles err = motionplan.CheckPlan( kb.Kinematics(), @@ -385,8 +384,6 @@ func (ms *explore) checkForObstacles( lookAheadDistanceMM, ms.logger, ) - fmt.Println("HELLLL YEA WE MADE IT OUT OF THE FUNCTION") - fmt.Println("err: ", err) if err != nil { if strings.Contains(err.Error(), "found constraint violation or collision") { ms.logger.CDebug(ctx, "collision found in given range") From 21cdb8e5da9d1d28eaead5dbbca924c850705e00 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 17 May 2024 16:28:13 -0400 Subject: [PATCH 048/126] add back space --- services/motion/explore/explore.go | 1 + 1 file changed, 1 insertion(+) diff --git a/services/motion/explore/explore.go b/services/motion/explore/explore.go index 0ab178e1256..f5dacc5ebe0 100644 --- a/services/motion/explore/explore.go +++ b/services/motion/explore/explore.go @@ -375,6 +375,7 @@ func (ms *explore) checkForObstacles( } errCounterCurrentInputs++ } + // Check plan for transient obstacles err = motionplan.CheckPlan( kb.Kinematics(), From 43f910dcf3517327685a28e10c255455e45ad1fe Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 20 May 2024 16:48:33 -0400 Subject: [PATCH 049/126] re-root FS in plan-manager --- motionplan/planManager.go | 29 +++++++------------------ services/motion/builtin/move_request.go | 29 +++++++++++++++++++------ 2 files changed, 30 insertions(+), 28 deletions(-) diff --git a/motionplan/planManager.go b/motionplan/planManager.go index c5b57f6bfa6..3484a8fd5d0 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -788,35 +788,22 @@ func (pm *planManager) planRelativeWaypoint(ctx context.Context, request *PlanRe } goalPos := tf.(*referenceframe.PoseInFrame).Pose() - // We need to create a separate ephemeral framesystem to place the - // kinematic base frame which only understands relative inputs, i.e. - // given what is provided to us by request.StartConfiguration cannot place us - // anywhere else but the origin - copyOfOriginalFS := pm.frame.fss - - transformFrame, err := referenceframe.NewStaticFrame(request.Frame.Name()+"ExecutionFrame", request.StartPose) - if err != nil { - return nil, err - } - fsWithTransform := referenceframe.NewEmptyFrameSystem("store-starting point") - err = fsWithTransform.AddFrame(transformFrame, fsWithTransform.World()) - if err != nil { - return nil, err - } - err = fsWithTransform.MergeFrameSystem(pm.frame.fss, transformFrame) + opt, err := pm.plannerSetupFromMoveRequest( + startPose, goalPos, request.StartConfiguration, request.WorldState, request.ConstraintSpecs, request.Options, + ) if err != nil { return nil, err } - pm.frame.fss = fsWithTransform - opt, err := pm.plannerSetupFromMoveRequest( - startPose, goalPos, request.StartConfiguration, request.WorldState, request.ConstraintSpecs, request.Options, - ) + // 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 = copyOfOriginalFS + pm.frame.fss = relativeOnlyFS pm.planOpts = opt + + // set the goal pose opt.SetGoal(goalPos) // Build planner diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 7477caa1d1a..a7fbfc4347e 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -653,6 +653,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 { @@ -669,22 +675,31 @@ func (ms *builtIn) createBaseMoveRequest( return nil, err } - // create checking fs - collisionFS := referenceframe.NewEmptyFrameSystem("collisionFS") - err = collisionFS.AddFrame(kb.ExecutionFrame(), collisionFS.World()) + // create fs used for planning + planningFS := referenceframe.NewEmptyFrameSystem("store-starting point") + transformFrame, err := referenceframe.NewStaticFrame(kb.Kinematics().Name()+"ExecutionFrame", startPose) if err != nil { return nil, err } - err = collisionFS.MergeFrameSystem(baseOnlyFS, kb.ExecutionFrame()) + err = planningFS.AddFrame(transformFrame, planningFS.World()) + if err != nil { + return nil, err + } + err = planningFS.MergeFrameSystem(baseOnlyFS, transformFrame) if err != nil { return nil, err } - startPoseIF, err := kb.CurrentPosition(ctx) + // create collision checking fs + collisionFS := referenceframe.NewEmptyFrameSystem("collisionFS") + err = collisionFS.AddFrame(kb.ExecutionFrame(), collisionFS.World()) + if err != nil { + return nil, err + } + err = collisionFS.MergeFrameSystem(baseOnlyFS, kb.ExecutionFrame()) if err != nil { return nil, err } - startPose := startPoseIF.Pose() goal := referenceframe.NewPoseInFrame(referenceframe.World, goalPoseInWorld) @@ -752,7 +767,7 @@ func (ms *builtIn) createBaseMoveRequest( Logger: logger, Goal: goal, Frame: kinematicFrame, - FrameSystem: baseOnlyFS, + FrameSystem: planningFS, StartConfiguration: currentInputs, StartPose: startPose, WorldState: worldState, From dbf6c190fbc0cf217dcd5c33d615fd8928243fc3 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Tue, 21 May 2024 12:14:08 -0400 Subject: [PATCH 050/126] replace ExecutionFrame with LocalizationFrame --- .../base/kinematicbase/differentialDrive.go | 24 +++--- .../kinematicbase/differentialDrive_test.go | 4 +- .../base/kinematicbase/fake_kinematics.go | 80 +++++++++---------- components/base/kinematicbase/kinematics.go | 2 +- .../base/kinematicbase/ptgKinematics.go | 28 +++---- motionplan/check.go | 12 +-- motionplan/tpSpaceRRT_test.go | 22 ++--- services/motion/builtin/move_on_map_test.go | 4 +- services/motion/builtin/move_request.go | 24 +++--- 9 files changed, 100 insertions(+), 100 deletions(-) diff --git a/components/base/kinematicbase/differentialDrive.go b/components/base/kinematicbase/differentialDrive.go index 59db958eb53..5ad84048e64 100644 --- a/components/base/kinematicbase/differentialDrive.go +++ b/components/base/kinematicbase/differentialDrive.go @@ -78,7 +78,7 @@ func wrapWithDifferentialDriveKinematics( } } - 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 +89,7 @@ func wrapWithDifferentialDriveKinematics( return nil, err } } else { - ddk.planningFrame = ddk.executionFrame + ddk.planningFrame = ddk.localizationFrame } ddk.noLocalizerCacheInputs = originInputs @@ -99,21 +99,21 @@ 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.Model + 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) ExecutionFrame() referenceframe.Frame { - return ddk.executionFrame +func (ddk *differentialDriveKinematics) LocalizationFrame() referenceframe.Frame { + return ddk.localizationFrame } func (ddk *differentialDriveKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { @@ -314,7 +314,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 cf5f42e0488..7e2ffbac955 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/fake_kinematics.go b/components/base/kinematicbase/fake_kinematics.go index 408f04463b1..4f2610dde72 100644 --- a/components/base/kinematicbase/fake_kinematics.go +++ b/components/base/kinematicbase/fake_kinematics.go @@ -21,12 +21,12 @@ import ( type fakeDiffDriveKinematics struct { *fake.Base - parentFrame string - planningFrame, executionFrame referenceframe.Frame - inputs []referenceframe.Input - options Options - sensorNoise spatialmath.Pose - lock sync.RWMutex + parentFrame string + planningFrame, localizationFrame referenceframe.Frame + inputs []referenceframe.Input + options Options + sensorNoise spatialmath.Pose + lock sync.RWMutex } // WrapWithFakeDiffDriveKinematics creates a DiffDrive KinematicBase from the fake Base so that it satisfies the ModelFramer and @@ -58,7 +58,7 @@ func WrapWithFakeDiffDriveKinematics( geometry = fk.Base.Geometry[0] } - fk.executionFrame, err = referenceframe.New2DMobileModelFrame(b.Name().ShortName(), limits, geometry) + fk.localizationFrame, err = referenceframe.New2DMobileModelFrame(b.Name().ShortName(), limits, geometry) if err != nil { return nil, err } @@ -69,7 +69,7 @@ func WrapWithFakeDiffDriveKinematics( return nil, err } } else { - fk.planningFrame = fk.executionFrame + fk.planningFrame = fk.localizationFrame } fk.options = options @@ -80,8 +80,8 @@ func (fk *fakeDiffDriveKinematics) Kinematics() referenceframe.Frame { return fk.planningFrame } -func (fk *fakeDiffDriveKinematics) ExecutionFrame() referenceframe.Frame { - return fk.executionFrame +func (fk *fakeDiffDriveKinematics) LocalizationFrame() referenceframe.Frame { + return fk.localizationFrame } func (fk *fakeDiffDriveKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { @@ -127,19 +127,19 @@ func (fk *fakeDiffDriveKinematics) CurrentPosition(ctx context.Context) (*refere type fakePTGKinematics struct { *fake.Base - localizer motion.Localizer - planningFrame, executionFrame referenceframe.Frame - options Options - sensorNoise spatialmath.Pose - ptgs []tpspace.PTGSolver - currentInput []referenceframe.Input - currentIndex int - plan motionplan.Plan - origin *referenceframe.PoseInFrame - positionlock sync.RWMutex - inputLock sync.RWMutex - logger logging.Logger - sleepTime int + localizer motion.Localizer + planningFrame, localizationFrame referenceframe.Frame + options Options + sensorNoise spatialmath.Pose + ptgs []tpspace.PTGSolver + currentInput []referenceframe.Input + currentIndex int + plan motionplan.Plan + origin *referenceframe.PoseInFrame + positionlock sync.RWMutex + inputLock sync.RWMutex + logger logging.Logger + sleepTime int } // WrapWithFakePTGKinematics creates a PTG KinematicBase from the fake Base so that it satisfies the ModelFramer and InputEnabled @@ -199,9 +199,9 @@ func WrapWithFakePTGKinematics( return nil, err } - // construct execution frame - executionFrame, err := referenceframe.New2DMobileModelFrame( - b.Name().ShortName()+"ExecutionFrame", + // construct localization frame + localizationFrame, err := referenceframe.New2DMobileModelFrame( + b.Name().ShortName()+"LocalizationFrame", []referenceframe.Limit{ {Min: math.Inf(-1), Max: math.Inf(1)}, {Min: math.Inf(-1), Max: math.Inf(1)}, @@ -229,17 +229,17 @@ func WrapWithFakePTGKinematics( zeroPlan := motionplan.NewSimplePlan(path, traj) fk := &fakePTGKinematics{ - Base: b, - planningFrame: planningFrame, - executionFrame: executionFrame, - origin: origin, - ptgs: ptgs, - currentInput: zeroInput, - currentIndex: 0, - plan: zeroPlan, - sensorNoise: sensorNoise, - logger: logger, - sleepTime: sleepTime, + Base: b, + planningFrame: planningFrame, + localizationFrame: localizationFrame, + origin: origin, + ptgs: ptgs, + currentInput: zeroInput, + currentIndex: 0, + plan: zeroPlan, + sensorNoise: sensorNoise, + logger: logger, + sleepTime: sleepTime, } initLocalizer := &fakePTGKinematicsLocalizer{fk} fk.localizer = motion.TwoDLocalizer(initLocalizer) @@ -252,8 +252,8 @@ func (fk *fakePTGKinematics) Kinematics() referenceframe.Frame { return fk.planningFrame } -func (fk *fakePTGKinematics) ExecutionFrame() referenceframe.Frame { - return fk.executionFrame +func (fk *fakePTGKinematics) LocalizationFrame() referenceframe.Frame { + return fk.localizationFrame } func (fk *fakePTGKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { @@ -353,7 +353,7 @@ func (fk *fakePTGKinematics) ExecutionState(ctx context.Context) (motionplan.Exe fk.plan, fk.currentIndex, map[string][]referenceframe.Input{fk.Kinematics().Name(): fk.currentInput}, - map[string]*referenceframe.PoseInFrame{fk.ExecutionFrame().Name(): pos}, + map[string]*referenceframe.PoseInFrame{fk.LocalizationFrame().Name(): pos}, ) } diff --git a/components/base/kinematicbase/kinematics.go b/components/base/kinematicbase/kinematics.go index 8bcaca3094f..4cb174d9f29 100644 --- a/components/base/kinematicbase/kinematics.go +++ b/components/base/kinematicbase/kinematics.go @@ -24,7 +24,7 @@ type KinematicBase interface { referenceframe.InputEnabled Kinematics() referenceframe.Frame - ExecutionFrame() referenceframe.Frame + LocalizationFrame() referenceframe.Frame // ErrorState takes a complete motionplan, as well as the index of the currently-executing set of inputs, and computes the pose // difference between where the robot in fact is, and where it ought to be, i.e. PoseBetween(expected, actual) diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index 035c98ddf56..8747d4f6dbc 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -32,13 +32,13 @@ const ( type ptgBaseKinematics struct { base.Base motion.Localizer - logger logging.Logger - planningFrame, executionFrame referenceframe.Frame - ptgs []tpspace.PTGSolver - courseCorrectionIdx int - linVelocityMMPerSecond float64 - angVelocityDegsPerSecond float64 - nonzeroBaseTurningRadiusMeters float64 + logger logging.Logger + planningFrame, localizationFrame referenceframe.Frame + ptgs []tpspace.PTGSolver + courseCorrectionIdx int + linVelocityMMPerSecond float64 + angVelocityDegsPerSecond float64 + nonzeroBaseTurningRadiusMeters float64 // All changeable state of the base is here inputLock sync.RWMutex @@ -141,9 +141,9 @@ func wrapWithPTGKinematics( } startingState := baseState{currentInputs: zeroInput} - // construct executionFrame - executionFrame, err := referenceframe.New2DMobileModelFrame( - b.Name().ShortName()+"ExecutionFrame", + // construct localization frame + localizationFrame, err := referenceframe.New2DMobileModelFrame( + b.Name().ShortName()+"LocalizationFrame", []referenceframe.Limit{ {Min: math.Inf(-1), Max: math.Inf(1)}, {Min: math.Inf(-1), Max: math.Inf(1)}, @@ -160,7 +160,7 @@ func wrapWithPTGKinematics( Localizer: localizer, logger: logger, planningFrame: planningFrame, - executionFrame: executionFrame, + localizationFrame: localizationFrame, ptgs: ptgs, courseCorrectionIdx: courseCorrectionIdx, linVelocityMMPerSecond: linVelocityMMPerSecond, @@ -176,8 +176,8 @@ func (ptgk *ptgBaseKinematics) Kinematics() referenceframe.Frame { return ptgk.planningFrame } -func (ptgk *ptgBaseKinematics) ExecutionFrame() referenceframe.Frame { - return ptgk.executionFrame +func (ptgk *ptgBaseKinematics) LocalizationFrame() referenceframe.Frame { + return ptgk.localizationFrame } // For a ptgBaseKinematics, `CurrentInputs` returns inputs which reflect what the base is currently doing. @@ -214,7 +214,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.ExecutionFrame().Name(): actualPIF}, + map[string]*referenceframe.PoseInFrame{ptgk.LocalizationFrame().Name(): actualPIF}, ) } diff --git a/motionplan/check.go b/motionplan/check.go index 1b1dc678448..02fd56dbdee 100644 --- a/motionplan/check.go +++ b/motionplan/check.go @@ -125,10 +125,10 @@ func checkPlanRelative( if currentPoses == nil { return errors.New("executionState had nil return from CurrentPoses") } - executionFrameName := checkFrame.Name() + "ExecutionFrame" - currentPoseIF, ok := currentPoses[executionFrameName] + localizationFrameName := checkFrame.Name() + "LocalizationFrame" + currentPoseIF, ok := currentPoses[localizationFrameName] if !ok { - return errors.New("checkFrameExecutionFrame not found in current pose map") + return errors.New("checkFrameLocalizationFrame not found in current pose map") } sf := sfPlanner.frame @@ -213,7 +213,7 @@ func checkPlanRelative( return err } currentWayPointTraj := plan.Trajectory()[wayPointIdx] - currentWayPointTraj[checkFrame.Name()+"ExecutionFrame"] = referenceframe.FloatsToInputs([]float64{ + currentWayPointTraj[checkFrame.Name()+"LocalizationFrame"] = referenceframe.FloatsToInputs([]float64{ poses[wayPointIdx].Point().X, poses[wayPointIdx].Point().Y, poses[wayPointIdx].Orientation().OrientationVectorRadians().Theta, @@ -258,11 +258,11 @@ func checkPlanRelative( } } - startInputs[executionFrameName] = referenceframe.FloatsToInputs( + startInputs[localizationFrameName] = referenceframe.FloatsToInputs( []float64{lastArcEndPose.Point().X, lastArcEndPose.Point().Y, lastArcEndPose.Orientation().OrientationVectorRadians().Theta}, ) nextInputs := plan.Trajectory()[i] - nextInputs[executionFrameName] = startInputs[executionFrameName] + nextInputs[localizationFrameName] = startInputs[localizationFrameName] segment, err := createSegment(sf, lastArcEndPose, thisArcEndPose, startInputs, nextInputs) if err != nil { return err diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index d42413ec46d..bd370c37344 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -288,8 +288,8 @@ func TestPtgCheckPlan(t *testing.T) { // NOTE: WE NEED TO ADD AN EXECUTION FRAME TO THE CHECKING FRAMESYSTEM SINCE WE ONLY WANT TO RELY ON USING INPUTS tfFrameSystem := referenceframe.NewEmptyFrameSystem("transformFS") - executionFrame, err := referenceframe.New2DMobileModelFrame( - "ackframeExecutionFrame", []referenceframe.Limit{ + localizationFrame, err := referenceframe.New2DMobileModelFrame( + "ackframeLocalizationFrame", []referenceframe.Limit{ {Min: math.Inf(-1), Max: math.Inf(1)}, {Min: math.Inf(-1), Max: math.Inf(1)}, {Min: -360, Max: 360}, @@ -297,12 +297,12 @@ func TestPtgCheckPlan(t *testing.T) { ) test.That(t, err, test.ShouldBeNil) - err = tfFrameSystem.AddFrame(executionFrame, tfFrameSystem.World()) + err = tfFrameSystem.AddFrame(localizationFrame, tfFrameSystem.World()) test.That(t, err, test.ShouldBeNil) - err = tfFrameSystem.MergeFrameSystem(fs, executionFrame) + err = tfFrameSystem.MergeFrameSystem(fs, localizationFrame) test.That(t, err, test.ShouldBeNil) - inputs[executionFrame.Name()] = referenceframe.FloatsToInputs(make([]float64, len(executionFrame.DoF()))) + inputs[localizationFrame.Name()] = referenceframe.FloatsToInputs(make([]float64, len(localizationFrame.DoF()))) t.Run("base case - validate plan without obstacles", func(t *testing.T) { executionState := ExecutionState{ @@ -310,7 +310,7 @@ func TestPtgCheckPlan(t *testing.T) { index: 0, currentInputs: inputs, currentPose: map[string]*referenceframe.PoseInFrame{ - executionFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), + localizationFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), }, } err = CheckPlan(ackermanFrame, executionState, nil, tfFrameSystem, math.Inf(1), logger) @@ -332,7 +332,7 @@ func TestPtgCheckPlan(t *testing.T) { index: 0, currentInputs: inputs, currentPose: map[string]*referenceframe.PoseInFrame{ - executionFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), + localizationFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), }, } err = CheckPlan(ackermanFrame, executionState, worldState, tfFrameSystem, math.Inf(1), logger) @@ -378,7 +378,7 @@ func TestPtgCheckPlan(t *testing.T) { index: 1, currentInputs: inputs, // zero'd inputs are incorrect here currentPose: map[string]*referenceframe.PoseInFrame{ - executionFrame.Name(): plan.Path()[1][ackermanFrame.Name()], + localizationFrame.Name(): plan.Path()[1][ackermanFrame.Name()], }, } err = CheckPlan(ackermanFrame, executionState, worldState, tfFrameSystem, math.Inf(1), logger) @@ -402,7 +402,7 @@ func TestPtgCheckPlan(t *testing.T) { index: 1, currentInputs: inputs, currentPose: map[string]*referenceframe.PoseInFrame{ - executionFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), + localizationFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), }, } err = CheckPlan(ackermanFrame, executionState, worldState, tfFrameSystem, math.Inf(1), logger) @@ -435,7 +435,7 @@ func TestPtgCheckPlan(t *testing.T) { index: 2, currentInputs: inputs, currentPose: map[string]*referenceframe.PoseInFrame{ - executionFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), + localizationFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), }, } err = CheckPlan(ackermanFrame, executionState, worldState, tfFrameSystem, math.Inf(1), logger) @@ -462,7 +462,7 @@ func TestPtgCheckPlan(t *testing.T) { index: 2, currentInputs: inputs, currentPose: map[string]*referenceframe.PoseInFrame{ - executionFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), + localizationFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), }, } err = CheckPlan(ackermanFrame, executionState, worldState, tfFrameSystem, math.Inf(1), logger) diff --git a/services/motion/builtin/move_on_map_test.go b/services/motion/builtin/move_on_map_test.go index 178e2268567..508286c6b1c 100644 --- a/services/motion/builtin/move_on_map_test.go +++ b/services/motion/builtin/move_on_map_test.go @@ -613,7 +613,7 @@ func TestMoveOnMapStaticObs(t *testing.T) { ) test.That(t, err, test.ShouldBeNil) currentInputs := referenceframe.StartPositions(mr.absoluteFS) - currentInputs[mr.kinematicBase.ExecutionFrame().Name()] = referenceframe.FloatsToInputs( + currentInputs[mr.kinematicBase.LocalizationFrame().Name()] = referenceframe.FloatsToInputs( []float64{0.58772e3, -0.80826e3, 0}, ) executionState, err := motionplan.NewExecutionState( @@ -621,7 +621,7 @@ func TestMoveOnMapStaticObs(t *testing.T) { 1, currentInputs, map[string]*referenceframe.PoseInFrame{ - mr.kinematicBase.ExecutionFrame().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}, )), diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index a7fbfc4347e..5d0371eb387 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -186,10 +186,10 @@ func (mr *moveRequest) getTransientDetections( } inputMap := referenceframe.StartPositions(mr.absoluteFS) inputMap[mr.kinematicBase.Name().ShortName()] = baseExecutionState.CurrentInputs()[mr.kinematicBase.Name().ShortName()] - inputMap[mr.kinematicBase.ExecutionFrame().Name()] = referenceframe.FloatsToInputs([]float64{ - baseExecutionState.CurrentPoses()[mr.kinematicBase.ExecutionFrame().Name()].Pose().Point().X, - baseExecutionState.CurrentPoses()[mr.kinematicBase.ExecutionFrame().Name()].Pose().Point().Y, - baseExecutionState.CurrentPoses()[mr.kinematicBase.ExecutionFrame().Name()].Pose().Point().Z, + inputMap[mr.kinematicBase.LocalizationFrame().Name()] = referenceframe.FloatsToInputs([]float64{ + baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().X, + baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().Y, + baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().Z, }) detections, err := visSrvc.GetObjectPointClouds(ctx, camName.Name, nil) @@ -274,10 +274,10 @@ func (mr *moveRequest) obstaclesIntersectPlan( // configuration rather than the zero inputs inputMap := referenceframe.StartPositions(mr.absoluteFS) inputMap[mr.kinematicBase.Name().ShortName()] = baseExecutionState.CurrentInputs()[mr.kinematicBase.Name().ShortName()] - inputMap[mr.kinematicBase.ExecutionFrame().Name()] = referenceframe.FloatsToInputs([]float64{ - baseExecutionState.CurrentPoses()[mr.kinematicBase.ExecutionFrame().Name()].Pose().Point().X, - baseExecutionState.CurrentPoses()[mr.kinematicBase.ExecutionFrame().Name()].Pose().Point().Y, - baseExecutionState.CurrentPoses()[mr.kinematicBase.ExecutionFrame().Name()].Pose().Point().Z, + inputMap[mr.kinematicBase.LocalizationFrame().Name()] = referenceframe.FloatsToInputs([]float64{ + baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().X, + baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().Y, + baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().Z, }) executionState, err := motionplan.NewExecutionState( baseExecutionState.Plan(), @@ -290,7 +290,7 @@ func (mr *moveRequest) obstaclesIntersectPlan( } mr.logger.CDebugf(ctx, "CheckPlan inputs: \n currentPosition: %v\n currentInputs: %v\n worldstate: %s", - spatialmath.PoseToProtobuf(executionState.CurrentPoses()[mr.kinematicBase.ExecutionFrame().Name()].Pose()), + spatialmath.PoseToProtobuf(executionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose()), inputMap, worldState.String(), ) @@ -677,7 +677,7 @@ func (ms *builtIn) createBaseMoveRequest( // create fs used for planning planningFS := referenceframe.NewEmptyFrameSystem("store-starting point") - transformFrame, err := referenceframe.NewStaticFrame(kb.Kinematics().Name()+"ExecutionFrame", startPose) + transformFrame, err := referenceframe.NewStaticFrame(kb.Kinematics().Name()+"LocalizationFrame", startPose) if err != nil { return nil, err } @@ -692,11 +692,11 @@ func (ms *builtIn) createBaseMoveRequest( // create collision checking fs collisionFS := referenceframe.NewEmptyFrameSystem("collisionFS") - err = collisionFS.AddFrame(kb.ExecutionFrame(), collisionFS.World()) + err = collisionFS.AddFrame(kb.LocalizationFrame(), collisionFS.World()) if err != nil { return nil, err } - err = collisionFS.MergeFrameSystem(baseOnlyFS, kb.ExecutionFrame()) + err = collisionFS.MergeFrameSystem(baseOnlyFS, kb.LocalizationFrame()) if err != nil { return nil, err } From ce5077268c9499a1ffcd52a0bc04e807ff1840e7 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Tue, 21 May 2024 14:49:14 -0400 Subject: [PATCH 051/126] introduce Simple6DModel --- referenceframe/model.go | 91 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 91 insertions(+) diff --git a/referenceframe/model.go b/referenceframe/model.go index 431d9acb2b3..b79e8acec98 100644 --- a/referenceframe/model.go +++ b/referenceframe/model.go @@ -323,3 +323,94 @@ func New2DMobileModelFrame(name string, limits []Limit, collisionGeometry spatia } return model, nil } + +type Simple6DModel struct { + *baseFrame + modelConfig *ModelConfig + limits []Limit + collisionGeometry spatialmath.Geometry +} + +// NewSimpleModel constructs a new model. +func NewSimple6DModel(name string) *Simple6DModel { + return &Simple6DModel{} +} + +func New6DFrame(name string, limits []Limit, collisionGeometry spatialmath.Geometry) (Model, error) { + if len(limits) != 6 { + return nil, + errors.Errorf("Must have 6DOF state (x, y, OX, OY, OZ, theta) to create 6DFrame, have %d dof", len(limits)) + } + + model := NewSimple6DModel(name) + model.collisionGeometry = collisionGeometry + model.limits = limits + + return model, nil +} + +// Transform takes a model and a list of joint angles in radians and computes the dual quaternion representing the +// cartesian position of the end effector. This is useful for when conversions between quaternions and OV are not needed. +func (m *Simple6DModel) Transform(inputs []Input) (spatialmath.Pose, error) { + if len(inputs) != 6 { + return nil, NewIncorrectInputLengthError(len(inputs), 6) + } + return spatialmath.NewPose( + r3.Vector{X: inputs[0].Value, Y: inputs[1].Value, Z: 0}, + &spatialmath.OrientationVectorDegrees{ + OX: inputs[2].Value, + OY: inputs[3].Value, + OZ: inputs[4].Value, + Theta: inputs[5].Value, + }, + ), nil +} + +// Interpolate interpolates the given amount between the two sets of inputs. +func (m *Simple6DModel) Interpolate(from, to []Input, by float64) ([]Input, error) { + if len(from) != len(m.DoF()) { + return nil, NewIncorrectInputLengthError(len(from), len(m.DoF())) + } + if len(to) != len(m.DoF()) { + return nil, NewIncorrectInputLengthError(len(to), len(m.DoF())) + } + return interpolateInputs(from, to, by), nil +} + +// Geometries returns an object representing the 3D space associeted with the staticFrame. +func (m *Simple6DModel) Geometries(inputs []Input) (*GeometriesInFrame, error) { + transformByPose, err := m.Transform(inputs) + if err != nil { + return nil, err + } + return NewGeometriesInFrame(m.name, []spatialmath.Geometry{m.collisionGeometry.Transform(transformByPose)}), nil +} + +// DoF returns the number of degrees of freedom within a model. +func (m *Simple6DModel) DoF() []Limit { + return m.limits +} + +// ModelConfig returns the ModelConfig object used to create this model. +func (m *Simple6DModel) ModelConfig() *ModelConfig { + return m.modelConfig +} + +// MarshalJSON serializes a Model. +func (m *Simple6DModel) MarshalJSON() ([]byte, error) { + return json.Marshal(m.modelConfig) +} + +// InputFromProtobuf converts pb.JointPosition to inputs. +func (m *Simple6DModel) InputFromProtobuf(jp *pb.JointPositions) []Input { + inputs := make([]Input, 0, len(jp.Values)) + + return inputs +} + +// ProtobufFromInput converts inputs to pb.JointPosition. +func (m *Simple6DModel) ProtobufFromInput(input []Input) *pb.JointPositions { + jPos := &pb.JointPositions{} + + return jPos +} From ddbde4dc06d7a44e90146b2a36296c0c5768f948 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Tue, 21 May 2024 15:12:54 -0400 Subject: [PATCH 052/126] migrate simple6Dmodel to be a 7D model --- referenceframe/model.go | 46 +++++++++++++++++++++-------------------- 1 file changed, 24 insertions(+), 22 deletions(-) diff --git a/referenceframe/model.go b/referenceframe/model.go index b79e8acec98..05dd1063beb 100644 --- a/referenceframe/model.go +++ b/referenceframe/model.go @@ -324,25 +324,27 @@ func New2DMobileModelFrame(name string, limits []Limit, collisionGeometry spatia return model, nil } -type Simple6DModel struct { +// Simple7DModel TODO. +type Simple7DModel struct { *baseFrame modelConfig *ModelConfig limits []Limit collisionGeometry spatialmath.Geometry } -// NewSimpleModel constructs a new model. -func NewSimple6DModel(name string) *Simple6DModel { - return &Simple6DModel{} +// NewSimple7DModel constructs a new model. +func NewSimple7DModel(name string) *Simple7DModel { + return &Simple7DModel{} } +// New6DFrame TODO. func New6DFrame(name string, limits []Limit, collisionGeometry spatialmath.Geometry) (Model, error) { - if len(limits) != 6 { + if len(limits) != 7 { return nil, - errors.Errorf("Must have 6DOF state (x, y, OX, OY, OZ, theta) to create 6DFrame, have %d dof", len(limits)) + errors.Errorf("Must have 7DOF state (x, y, z, OX, OY, OZ, theta) to create 6DFrame, have %d dof", len(limits)) } - model := NewSimple6DModel(name) + model := NewSimple7DModel(name) model.collisionGeometry = collisionGeometry model.limits = limits @@ -351,23 +353,23 @@ func New6DFrame(name string, limits []Limit, collisionGeometry spatialmath.Geome // Transform takes a model and a list of joint angles in radians and computes the dual quaternion representing the // cartesian position of the end effector. This is useful for when conversions between quaternions and OV are not needed. -func (m *Simple6DModel) Transform(inputs []Input) (spatialmath.Pose, error) { - if len(inputs) != 6 { - return nil, NewIncorrectInputLengthError(len(inputs), 6) +func (m *Simple7DModel) Transform(inputs []Input) (spatialmath.Pose, error) { + if len(inputs) != 7 { + return nil, NewIncorrectInputLengthError(len(inputs), 7) } return spatialmath.NewPose( - r3.Vector{X: inputs[0].Value, Y: inputs[1].Value, Z: 0}, + r3.Vector{X: inputs[0].Value, Y: inputs[1].Value, Z: inputs[2].Value}, &spatialmath.OrientationVectorDegrees{ - OX: inputs[2].Value, - OY: inputs[3].Value, - OZ: inputs[4].Value, - Theta: inputs[5].Value, + 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 (m *Simple6DModel) Interpolate(from, to []Input, by float64) ([]Input, error) { +func (m *Simple7DModel) Interpolate(from, to []Input, by float64) ([]Input, error) { if len(from) != len(m.DoF()) { return nil, NewIncorrectInputLengthError(len(from), len(m.DoF())) } @@ -378,7 +380,7 @@ func (m *Simple6DModel) Interpolate(from, to []Input, by float64) ([]Input, erro } // Geometries returns an object representing the 3D space associeted with the staticFrame. -func (m *Simple6DModel) Geometries(inputs []Input) (*GeometriesInFrame, error) { +func (m *Simple7DModel) Geometries(inputs []Input) (*GeometriesInFrame, error) { transformByPose, err := m.Transform(inputs) if err != nil { return nil, err @@ -387,29 +389,29 @@ func (m *Simple6DModel) Geometries(inputs []Input) (*GeometriesInFrame, error) { } // DoF returns the number of degrees of freedom within a model. -func (m *Simple6DModel) DoF() []Limit { +func (m *Simple7DModel) DoF() []Limit { return m.limits } // ModelConfig returns the ModelConfig object used to create this model. -func (m *Simple6DModel) ModelConfig() *ModelConfig { +func (m *Simple7DModel) ModelConfig() *ModelConfig { return m.modelConfig } // MarshalJSON serializes a Model. -func (m *Simple6DModel) MarshalJSON() ([]byte, error) { +func (m *Simple7DModel) MarshalJSON() ([]byte, error) { return json.Marshal(m.modelConfig) } // InputFromProtobuf converts pb.JointPosition to inputs. -func (m *Simple6DModel) InputFromProtobuf(jp *pb.JointPositions) []Input { +func (m *Simple7DModel) InputFromProtobuf(jp *pb.JointPositions) []Input { inputs := make([]Input, 0, len(jp.Values)) return inputs } // ProtobufFromInput converts inputs to pb.JointPosition. -func (m *Simple6DModel) ProtobufFromInput(input []Input) *pb.JointPositions { +func (m *Simple7DModel) ProtobufFromInput(input []Input) *pb.JointPositions { jPos := &pb.JointPositions{} return jPos From 1d99d0c44ae15805277fc14ede324994cd3c6e26 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Tue, 21 May 2024 16:32:16 -0400 Subject: [PATCH 053/126] change functionaluty of bases to use 7Dof frame - partway through progress --- .../base/kinematicbase/fake_kinematics.go | 4 +-- .../base/kinematicbase/ptgKinematics.go | 8 ++++-- referenceframe/model.go | 2 +- services/motion/builtin/move_request.go | 27 ++++++++++++++++--- 4 files changed, 32 insertions(+), 9 deletions(-) diff --git a/components/base/kinematicbase/fake_kinematics.go b/components/base/kinematicbase/fake_kinematics.go index 415585f713e..d5082dcdf27 100644 --- a/components/base/kinematicbase/fake_kinematics.go +++ b/components/base/kinematicbase/fake_kinematics.go @@ -295,12 +295,12 @@ func (fk *fakePTGKinematics) GoToInputs(ctx context.Context, inputSteps ...[]ref fk.inputLock.Lock() fk.currentIndex = i - fk.currentInput, err = fk.frame.Interpolate(zeroInput, inputs, 0) + fk.currentInput, err = fk.planningFrame.Interpolate(zeroInput, inputs, 0) fk.inputLock.Unlock() if err != nil { return err } - finalPose, err := fk.frame.Transform(inputs) + finalPose, err := fk.planningFrame.Transform(inputs) if err != nil { return err } diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index 8747d4f6dbc..7937a0e6690 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -142,12 +142,16 @@ func wrapWithPTGKinematics( startingState := baseState{currentInputs: zeroInput} // construct localization frame - localizationFrame, err := referenceframe.New2DMobileModelFrame( + localizationFrame, err := referenceframe.New6DFrame( b.Name().ShortName()+"LocalizationFrame", []referenceframe.Limit{ {Min: math.Inf(-1), Max: math.Inf(1)}, {Min: math.Inf(-1), Max: math.Inf(1)}, - {Min: -360, Max: 360}, + {Min: math.Inf(-1), Max: math.Inf(1)}, + {Min: -1, Max: 1}, + {Min: -1, Max: 1}, + {Min: -1, Max: 1}, + {Min: -2 * math.Pi, Max: 2 * math.Pi}, }, nil, ) diff --git a/referenceframe/model.go b/referenceframe/model.go index 05dd1063beb..e1b975fe69c 100644 --- a/referenceframe/model.go +++ b/referenceframe/model.go @@ -359,7 +359,7 @@ func (m *Simple7DModel) Transform(inputs []Input) (spatialmath.Pose, error) { } return spatialmath.NewPose( r3.Vector{X: inputs[0].Value, Y: inputs[1].Value, Z: inputs[2].Value}, - &spatialmath.OrientationVectorDegrees{ + &spatialmath.OrientationVector{ OX: inputs[3].Value, OY: inputs[4].Value, OZ: inputs[5].Value, diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 5d0371eb387..4b43d1238dc 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -190,6 +190,10 @@ func (mr *moveRequest) getTransientDetections( baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().X, baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().Y, baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().Z, + baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().OX, + baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().OY, + baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().OZ, + baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().Theta, }) detections, err := visSrvc.GetObjectPointClouds(ctx, camName.Name, nil) @@ -278,6 +282,10 @@ func (mr *moveRequest) obstaclesIntersectPlan( baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().X, baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().Y, baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().Z, + baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().OX, + baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().OY, + baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().OZ, + baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().Theta, }) executionState, err := motionplan.NewExecutionState( baseExecutionState.Plan(), @@ -514,10 +522,17 @@ func (ms *builtIn) newMoveOnGlobeRequest( if straightlineDistance > maxTravelDistanceMM { return nil, fmt.Errorf("cannot move more than %d kilometers", int(maxTravelDistanceMM*1e-6)) } + + // these limits need to be updated so that they are in 7DOF + // what is a good limit for what OX, OY, OZ should be? limits := []referenceframe.Limit{ - {Min: -straightlineDistance * 3, Max: straightlineDistance * 3}, - {Min: -straightlineDistance * 3, Max: straightlineDistance * 3}, - {Min: -2 * math.Pi, Max: 2 * math.Pi}, + {Min: -straightlineDistance * 3, Max: straightlineDistance * 3}, // X + {Min: -straightlineDistance * 3, Max: straightlineDistance * 3}, // Y + {Min: -straightlineDistance * 3, Max: straightlineDistance * 3}, // Z + {Min: -1, Max: 1}, // OX + {Min: -1, Max: 1}, // OY + {Min: -1, Max: 1}, // OZ + {Min: -2 * math.Pi, Max: 2 * math.Pi}, // Theta } // Note: this is only for diff drive, not used for PTGs kb, err := kinematicbase.WrapWithKinematics(ctx, b, ms.logger, localizer, limits, kinematicsOptions) if err != nil { @@ -584,7 +599,11 @@ func (ms *builtIn) newMoveOnMapRequest( if err != nil { return nil, err } - limits = append(limits, referenceframe.Limit{Min: -2 * math.Pi, Max: 2 * math.Pi}) + limits = append(limits, referenceframe.Limit{Min: -1, Max: 1}) // Z + limits = append(limits, referenceframe.Limit{Min: -1, Max: 1}) // OX + limits = append(limits, referenceframe.Limit{Min: -1, Max: 1}) // OY + limits = append(limits, referenceframe.Limit{Min: -1, Max: 1}) // OZ + limits = append(limits, referenceframe.Limit{Min: -2 * math.Pi, Max: 2 * math.Pi}) // Theta // create a KinematicBase from the componentName component, ok := ms.components[req.ComponentName] From 81182066289f71134e308f4c24dda471a35c267e Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 23 May 2024 14:21:45 -0400 Subject: [PATCH 054/126] battle-testing use of 7DoF frame for checking plan - tests still failing --- .../base/kinematicbase/fake_kinematics.go | 8 +- .../base/kinematicbase/ptgKinematics.go | 4 +- motionplan/check.go | 18 +++- motionplan/motionPlanner.go | 3 +- motionplan/planManager.go | 8 ++ referenceframe/frame.go | 94 +++++++++++++++++++ referenceframe/model.go | 93 ------------------ services/motion/builtin/builtin_test.go | 74 +++++++-------- services/motion/builtin/move_on_map_test.go | 2 +- services/motion/builtin/move_request.go | 26 ++++- 10 files changed, 189 insertions(+), 141 deletions(-) diff --git a/components/base/kinematicbase/fake_kinematics.go b/components/base/kinematicbase/fake_kinematics.go index d5082dcdf27..ca1671a2f4b 100644 --- a/components/base/kinematicbase/fake_kinematics.go +++ b/components/base/kinematicbase/fake_kinematics.go @@ -200,12 +200,16 @@ func WrapWithFakePTGKinematics( } // construct localization frame - localizationFrame, err := referenceframe.New2DMobileModelFrame( + localizationFrame, err := referenceframe.New7DFrame( b.Name().ShortName()+"LocalizationFrame", []referenceframe.Limit{ {Min: math.Inf(-1), Max: math.Inf(1)}, {Min: math.Inf(-1), Max: math.Inf(1)}, - {Min: -360, Max: 360}, + {Min: math.Inf(-1), Max: math.Inf(1)}, + {Min: -1, Max: 1}, + {Min: -1, Max: 1}, + {Min: -1, Max: 1}, + {Min: -2 * math.Pi, Max: 2 * math.Pi}, }, nil, ) diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index 699919e553e..ef7c0810c81 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -140,9 +140,8 @@ func wrapWithPTGKinematics( origin = originPIF.Pose() } startingState := baseState{currentInputs: zeroInput} - // construct localization frame - localizationFrame, err := referenceframe.New6DFrame( + localizationFrame, err := referenceframe.New7DFrame( b.Name().ShortName()+"LocalizationFrame", []referenceframe.Limit{ {Min: math.Inf(-1), Max: math.Inf(1)}, @@ -192,7 +191,6 @@ func (ptgk *ptgBaseKinematics) CurrentInputs(ctx context.Context) ([]referencefr ptgk.inputLock.RLock() defer ptgk.inputLock.RUnlock() - // ptgk.Localizer. planningFrameInputs := ptgk.currentState.currentInputs planningFrameInputs = append(planningFrameInputs, []referenceframe.Input{}...) return planningFrameInputs, nil diff --git a/motionplan/check.go b/motionplan/check.go index 02fd56dbdee..2eec6020747 100644 --- a/motionplan/check.go +++ b/motionplan/check.go @@ -216,6 +216,10 @@ func checkPlanRelative( currentWayPointTraj[checkFrame.Name()+"LocalizationFrame"] = referenceframe.FloatsToInputs([]float64{ poses[wayPointIdx].Point().X, poses[wayPointIdx].Point().Y, + poses[wayPointIdx].Point().Z, + poses[wayPointIdx].Orientation().OrientationVectorRadians().OX, + poses[wayPointIdx].Orientation().OrientationVectorRadians().OY, + poses[wayPointIdx].Orientation().OrientationVectorRadians().OZ, poses[wayPointIdx].Orientation().OrientationVectorRadians().Theta, }) @@ -259,7 +263,13 @@ func checkPlanRelative( } startInputs[localizationFrameName] = referenceframe.FloatsToInputs( - []float64{lastArcEndPose.Point().X, lastArcEndPose.Point().Y, lastArcEndPose.Orientation().OrientationVectorRadians().Theta}, + []float64{ + lastArcEndPose.Point().X, lastArcEndPose.Point().Y, lastArcEndPose.Point().Z, + lastArcEndPose.Orientation().OrientationVectorRadians().OX, + lastArcEndPose.Orientation().OrientationVectorRadians().OY, + lastArcEndPose.Orientation().OrientationVectorRadians().OZ, + lastArcEndPose.Orientation().OrientationVectorRadians().Theta, + }, ) nextInputs := plan.Trajectory()[i] nextInputs[localizationFrameName] = startInputs[localizationFrameName] @@ -270,6 +280,11 @@ func checkPlanRelative( lastArcEndPose = thisArcEndPose segments = append(segments, segment) } + + // fmt.Println("PRINTING ALL SEGMENTS BELOW ") + // for _, s := range segments { + // fmt.Println(s) + // } return checkSegments(sfPlanner, segments, lookAheadDistanceMM) } @@ -385,6 +400,7 @@ func checkSegments(sfPlanner *planManager, segments []*ik.Segment, lookAheadDist if err != nil { return err } + fmt.Println("poseInPath: ", spatialmath.PoseToProtobuf(poseInPath)) // Check if look ahead distance has been reached currentTravelDistanceMM := totalTravelDistanceMM + poseInPath.Point().Distance(segment.StartPosition.Point()) diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 867c6bfef17..d8763041ca3 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -5,6 +5,7 @@ package motionplan import ( "context" + "fmt" "math/rand" "sort" "sync" @@ -178,7 +179,7 @@ func Replan(ctx context.Context, request *PlanRequest, currentPlan Plan, replanC return nil, errHighReplanCost } } - + fmt.Println(newPlan.Path()) return newPlan, nil } diff --git a/motionplan/planManager.go b/motionplan/planManager.go index 3484a8fd5d0..0efcac1e17f 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -503,6 +503,10 @@ func (pm *planManager) plannerSetupFromMoveRequest( return nil, err // no geometries defined for frame } movingRobotGeometries := movingGeometriesInFrame.Geometries() // solver frame returns geoms in frame World + // fmt.Println("PRINTING movingRobotGeometries") + // for _, g := range movingRobotGeometries { + // fmt.Println("g.Pose: ", spatialmath.PoseToProtobuf(g.Pose())) + // } // find all geometries that are not moving but are in the frame system staticRobotGeometries := make([]spatialmath.Geometry, 0) @@ -523,6 +527,10 @@ func (pm *planManager) plannerSetupFromMoveRequest( if err != nil { return nil, err } + fmt.Println("ABOUT TO PRINT WORLD GEOMETRIES ") + for _, g := range worldGeometries.Geometries() { + fmt.Println("g.Pose: ", spatialmath.PoseToProtobuf(g.Pose())) + } allowedCollisions, err := collisionSpecificationsFromProto(constraints.GetCollisionSpecification(), frameSystemGeometries, worldState) if err != nil { diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 0fe4d65e9a3..cbfa391aa2d 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -445,3 +445,97 @@ func (rf rotationalFrame) MarshalJSON() ([]byte, error) { return json.Marshal(temp) } + +type sevenDFrame struct { + *baseFrame + collisionGeometry spatial.Geometry +} + +// New6DFrame TODO. +func New7DFrame(name string, limits []Limit, collisionGeometry spatial.Geometry) (Frame, error) { + if len(limits) != 7 { + return nil, + errors.Errorf("Must have 7DOF state (x, y, z, OX, OY, OZ, theta) to create 6DFrame, have %d dof", len(limits)) + } + + return &sevenDFrame{ + &baseFrame{name, limits}, + collisionGeometry, + }, nil + +} + +// Transform takes a model and a list of joint angles in radians and computes the dual quaternion representing the +// cartesian position of the end effector. This is useful for when conversions between quaternions and OV are not needed. +func (m *sevenDFrame) Transform(inputs []Input) (spatial.Pose, error) { + if len(inputs) != 7 { + return nil, NewIncorrectInputLengthError(len(inputs), 7) + } + 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 (m *sevenDFrame) Interpolate(from, to []Input, by float64) ([]Input, error) { + if len(from) != len(m.DoF()) { + return nil, NewIncorrectInputLengthError(len(from), len(m.DoF())) + } + if len(to) != len(m.DoF()) { + return nil, NewIncorrectInputLengthError(len(to), len(m.DoF())) + } + return interpolateInputs(from, to, by), nil +} + +// Geometries returns an object representing the 3D space associeted with the staticFrame. +func (m *sevenDFrame) Geometries(inputs []Input) (*GeometriesInFrame, error) { + transformByPose, err := m.Transform(inputs) + if err != nil { + return nil, err + } + if m.collisionGeometry == nil { + return NewGeometriesInFrame(m.name, []spatial.Geometry{}), nil + } + return NewGeometriesInFrame(m.name, []spatial.Geometry{m.collisionGeometry.Transform(transformByPose)}), nil +} + +// DoF returns the number of degrees of freedom within a model. +func (m *sevenDFrame) DoF() []Limit { + return m.limits +} + +// MarshalJSON serializes a Model. +func (m *sevenDFrame) MarshalJSON() ([]byte, error) { + temp := LinkConfig{ + ID: m.name, + } + + if m.collisionGeometry != nil { + var err error + temp.Geometry, err = spatial.NewGeometryConfig(m.collisionGeometry) + if err != nil { + return nil, err + } + } + return json.Marshal(temp) +} + +// InputFromProtobuf converts pb.JointPosition to inputs. +func (m *sevenDFrame) InputFromProtobuf(jp *pb.JointPositions) []Input { + inputs := make([]Input, 0, len(jp.Values)) + + return inputs +} + +// ProtobufFromInput converts inputs to pb.JointPosition. +func (m *sevenDFrame) ProtobufFromInput(input []Input) *pb.JointPositions { + jPos := &pb.JointPositions{} + + return jPos +} diff --git a/referenceframe/model.go b/referenceframe/model.go index e1b975fe69c..431d9acb2b3 100644 --- a/referenceframe/model.go +++ b/referenceframe/model.go @@ -323,96 +323,3 @@ func New2DMobileModelFrame(name string, limits []Limit, collisionGeometry spatia } return model, nil } - -// Simple7DModel TODO. -type Simple7DModel struct { - *baseFrame - modelConfig *ModelConfig - limits []Limit - collisionGeometry spatialmath.Geometry -} - -// NewSimple7DModel constructs a new model. -func NewSimple7DModel(name string) *Simple7DModel { - return &Simple7DModel{} -} - -// New6DFrame TODO. -func New6DFrame(name string, limits []Limit, collisionGeometry spatialmath.Geometry) (Model, error) { - if len(limits) != 7 { - return nil, - errors.Errorf("Must have 7DOF state (x, y, z, OX, OY, OZ, theta) to create 6DFrame, have %d dof", len(limits)) - } - - model := NewSimple7DModel(name) - model.collisionGeometry = collisionGeometry - model.limits = limits - - return model, nil -} - -// Transform takes a model and a list of joint angles in radians and computes the dual quaternion representing the -// cartesian position of the end effector. This is useful for when conversions between quaternions and OV are not needed. -func (m *Simple7DModel) Transform(inputs []Input) (spatialmath.Pose, error) { - if len(inputs) != 7 { - return nil, NewIncorrectInputLengthError(len(inputs), 7) - } - return spatialmath.NewPose( - r3.Vector{X: inputs[0].Value, Y: inputs[1].Value, Z: inputs[2].Value}, - &spatialmath.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 (m *Simple7DModel) Interpolate(from, to []Input, by float64) ([]Input, error) { - if len(from) != len(m.DoF()) { - return nil, NewIncorrectInputLengthError(len(from), len(m.DoF())) - } - if len(to) != len(m.DoF()) { - return nil, NewIncorrectInputLengthError(len(to), len(m.DoF())) - } - return interpolateInputs(from, to, by), nil -} - -// Geometries returns an object representing the 3D space associeted with the staticFrame. -func (m *Simple7DModel) Geometries(inputs []Input) (*GeometriesInFrame, error) { - transformByPose, err := m.Transform(inputs) - if err != nil { - return nil, err - } - return NewGeometriesInFrame(m.name, []spatialmath.Geometry{m.collisionGeometry.Transform(transformByPose)}), nil -} - -// DoF returns the number of degrees of freedom within a model. -func (m *Simple7DModel) DoF() []Limit { - return m.limits -} - -// ModelConfig returns the ModelConfig object used to create this model. -func (m *Simple7DModel) ModelConfig() *ModelConfig { - return m.modelConfig -} - -// MarshalJSON serializes a Model. -func (m *Simple7DModel) MarshalJSON() ([]byte, error) { - return json.Marshal(m.modelConfig) -} - -// InputFromProtobuf converts pb.JointPosition to inputs. -func (m *Simple7DModel) InputFromProtobuf(jp *pb.JointPositions) []Input { - inputs := make([]Input, 0, len(jp.Values)) - - return inputs -} - -// ProtobufFromInput converts inputs to pb.JointPosition. -func (m *Simple7DModel) ProtobufFromInput(input []Input) *pb.JointPositions { - jPos := &pb.JointPositions{} - - return jPos -} diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 55e0d83033a..0f75dff259c 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -334,46 +334,46 @@ func TestObstacleReplanningGlobe(t *testing.T) { } extra := map[string]interface{}{"max_replans": 10, "max_ik_solutions": 1, "smooth_iter": 1} - extraNoReplan := map[string]interface{}{"max_replans": 0, "max_ik_solutions": 1, "smooth_iter": 1} + // extraNoReplan := map[string]interface{}{"max_replans": 0, "max_ik_solutions": 1, "smooth_iter": 1} // We set a flag here per test case so that detections are not returned the first time each vision service is called testCases := []testCase{ - { - name: "ensure no replan from discovered obstacles", - getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { - caseName := "test-case-1" - obstaclePosition := spatialmath.NewPoseFromPoint(r3.Vector{X: -1000, Y: -1000, Z: 0}) - box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 10, Y: 10, Z: 10}, caseName) - test.That(t, err, test.ShouldBeNil) - - detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) - test.That(t, err, test.ShouldBeNil) - - return []*viz.Object{detection}, nil - }, - expectedSuccess: true, - extra: extraNoReplan, - }, - { - name: "ensure replan due to obstacle collision", - getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { - caseName := "test-case-2" - // The camera is parented to the base. Thus, this will always see an obstacle 300mm in front of where the base is. - // Note: for createMoveOnGlobeEnvironment, 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) - test.That(t, err, test.ShouldBeNil) - - detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) - test.That(t, err, test.ShouldBeNil) - - return []*viz.Object{detection}, nil - }, - expectedSuccess: false, - expectedErr: fmt.Sprintf("exceeded maximum number of replans: %d: plan failed", 0), - extra: extraNoReplan, - }, + // { + // name: "ensure no replan from discovered obstacles", + // getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { + // caseName := "test-case-1" + // obstaclePosition := spatialmath.NewPoseFromPoint(r3.Vector{X: -1000, Y: -1000, Z: 0}) + // box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 10, Y: 10, Z: 10}, caseName) + // test.That(t, err, test.ShouldBeNil) + + // detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) + // test.That(t, err, test.ShouldBeNil) + + // return []*viz.Object{detection}, nil + // }, + // expectedSuccess: true, + // extra: extraNoReplan, + // }, + // { + // name: "ensure replan due to obstacle collision", + // getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { + // caseName := "test-case-2" + // // The camera is parented to the base. Thus, this will always see an obstacle 300mm in front of where the base is. + // // Note: for createMoveOnGlobeEnvironment, 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) + // test.That(t, err, test.ShouldBeNil) + + // detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) + // test.That(t, err, test.ShouldBeNil) + + // return []*viz.Object{detection}, nil + // }, + // expectedSuccess: false, + // expectedErr: fmt.Sprintf("exceeded maximum number of replans: %d: plan failed", 0), + // extra: extraNoReplan, + // }, { name: "ensure replan reaching goal", getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { diff --git a/services/motion/builtin/move_on_map_test.go b/services/motion/builtin/move_on_map_test.go index 1dc41a36ff8..66e523d21bb 100644 --- a/services/motion/builtin/move_on_map_test.go +++ b/services/motion/builtin/move_on_map_test.go @@ -591,7 +591,7 @@ func TestMoveOnMapStaticObs(t *testing.T) { test.That(t, err, test.ShouldBeNil) currentInputs := referenceframe.StartPositions(mr.absoluteFS) currentInputs[mr.kinematicBase.LocalizationFrame().Name()] = referenceframe.FloatsToInputs( - []float64{0.58772e3, -0.80826e3, 0}, + []float64{0.58772e3, -0.80826e3, 0, 0, 0, 1, 0}, ) executionState, err := motionplan.NewExecutionState( plan, diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 4b43d1238dc..54541f8da58 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -185,7 +185,11 @@ func (mr *moveRequest) getTransientDetections( return nil, err } inputMap := referenceframe.StartPositions(mr.absoluteFS) - inputMap[mr.kinematicBase.Name().ShortName()] = baseExecutionState.CurrentInputs()[mr.kinematicBase.Name().ShortName()] + + fmt.Println("baseExecutionState.CurrentInputs(): ", baseExecutionState.CurrentInputs()) + fmt.Println("baseExecutionState.CurrentPoses(): ", baseExecutionState.CurrentPoses()) + + inputMap[mr.kinematicBase.Name().ShortName()] = make([]referenceframe.Input, len(mr.kinematicBase.Kinematics().DoF())) inputMap[mr.kinematicBase.LocalizationFrame().Name()] = referenceframe.FloatsToInputs([]float64{ baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().X, baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().Y, @@ -195,11 +199,14 @@ func (mr *moveRequest) getTransientDetections( baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().OZ, baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().Theta, }) + // fmt.Println("do i get to this point in time though") detections, err := visSrvc.GetObjectPointClouds(ctx, camName.Name, nil) if err != nil { return nil, err } + fmt.Println("inputMap: ", inputMap) + // fmt.Println("mr.kinematicBase.LocalizationFrame().DoF(): ", mr.kinematicBase.LocalizationFrame().DoF()) // transformed detections transientGeoms := []spatialmath.Geometry{} @@ -210,6 +217,7 @@ func (mr *moveRequest) getTransientDetections( if geometry.Label() != "" { label += "_" + geometry.Label() } + fmt.Println("current geometry pose: ", spatialmath.PoseToProtobuf(geometry.Pose())) geometry.SetLabel(label) tf, err := mr.absoluteFS.Transform( inputMap, @@ -223,6 +231,12 @@ func (mr *moveRequest) getTransientDetections( if !ok { return nil, errors.New("unable to assert referenceframe.Transformable into *referenceframe.GeometriesInFrame") } + fmt.Println("ABOUT TO PRINT WORLD GIF GEOMETRIES") + for _, g := range worldGifs.Geometries() { + fmt.Println("g.Pose: ", spatialmath.PoseToProtobuf(g.Pose())) + + } + fmt.Println(" ") transientGeoms = append(transientGeoms, worldGifs.Geometries()...) } return referenceframe.NewGeometriesInFrame(referenceframe.World, transientGeoms), nil @@ -256,6 +270,7 @@ func (mr *moveRequest) obstaclesIntersectPlan( if err != nil { return state.ExecuteResponse{}, err } + // fmt.Println("do i make it here though?") if len(gifs.Geometries()) == 0 { mr.logger.CDebug(ctx, "will not check if obstacles intersect path since nothing was detected") continue @@ -277,7 +292,8 @@ func (mr *moveRequest) obstaclesIntersectPlan( // 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.absoluteFS) - inputMap[mr.kinematicBase.Name().ShortName()] = baseExecutionState.CurrentInputs()[mr.kinematicBase.Name().ShortName()] + // inputMap[mr.kinematicBase.Name().ShortName()] = baseExecutionState.CurrentInputs()[mr.kinematicBase.Name().ShortName()] + inputMap[mr.kinematicBase.Name().ShortName()] = make([]referenceframe.Input, len(mr.kinematicBase.Kinematics().DoF())) inputMap[mr.kinematicBase.LocalizationFrame().Name()] = referenceframe.FloatsToInputs([]float64{ baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().X, baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().Y, @@ -296,7 +312,7 @@ func (mr *moveRequest) obstaclesIntersectPlan( if err != nil { return state.ExecuteResponse{}, err } - + // fmt.Println("PRINTING CHECK PLAN INPUTS BELOW") mr.logger.CDebugf(ctx, "CheckPlan inputs: \n currentPosition: %v\n currentInputs: %v\n worldstate: %s", spatialmath.PoseToProtobuf(executionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose()), inputMap, @@ -311,9 +327,13 @@ func (mr *moveRequest) obstaclesIntersectPlan( lookAheadDistanceMM, mr.planRequest.Logger, ); err != nil { + fmt.Println("WE ARE RETURNING AN ERROR") + fmt.Println(" ") mr.planRequest.Logger.CInfo(ctx, err.Error()) return state.ExecuteResponse{Replan: true, ReplanReason: err.Error()}, nil } + fmt.Println("THERE WAS NO ERROR HERE") + fmt.Println(" ") } } return state.ExecuteResponse{}, nil From 21c8a4f8cce8fa2e5513884a06c99ef985400d9c Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 23 May 2024 18:02:46 -0400 Subject: [PATCH 055/126] tests passing for 7DoF frame --- motionplan/check.go | 5 -- motionplan/motionPlanner.go | 3 +- motionplan/planManager.go | 8 --- motionplan/tpSpaceRRT_test.go | 16 ++++-- referenceframe/frame.go | 3 +- services/motion/builtin/builtin_test.go | 76 ++++++++++++------------- services/motion/builtin/move_request.go | 61 ++++++-------------- 7 files changed, 69 insertions(+), 103 deletions(-) diff --git a/motionplan/check.go b/motionplan/check.go index 2eec6020747..30b9739dff4 100644 --- a/motionplan/check.go +++ b/motionplan/check.go @@ -281,10 +281,6 @@ func checkPlanRelative( segments = append(segments, segment) } - // fmt.Println("PRINTING ALL SEGMENTS BELOW ") - // for _, s := range segments { - // fmt.Println(s) - // } return checkSegments(sfPlanner, segments, lookAheadDistanceMM) } @@ -400,7 +396,6 @@ func checkSegments(sfPlanner *planManager, segments []*ik.Segment, lookAheadDist if err != nil { return err } - fmt.Println("poseInPath: ", spatialmath.PoseToProtobuf(poseInPath)) // Check if look ahead distance has been reached currentTravelDistanceMM := totalTravelDistanceMM + poseInPath.Point().Distance(segment.StartPosition.Point()) diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index d8763041ca3..867c6bfef17 100644 --- a/motionplan/motionPlanner.go +++ b/motionplan/motionPlanner.go @@ -5,7 +5,6 @@ package motionplan import ( "context" - "fmt" "math/rand" "sort" "sync" @@ -179,7 +178,7 @@ func Replan(ctx context.Context, request *PlanRequest, currentPlan Plan, replanC return nil, errHighReplanCost } } - fmt.Println(newPlan.Path()) + return newPlan, nil } diff --git a/motionplan/planManager.go b/motionplan/planManager.go index 0efcac1e17f..3484a8fd5d0 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -503,10 +503,6 @@ func (pm *planManager) plannerSetupFromMoveRequest( return nil, err // no geometries defined for frame } movingRobotGeometries := movingGeometriesInFrame.Geometries() // solver frame returns geoms in frame World - // fmt.Println("PRINTING movingRobotGeometries") - // for _, g := range movingRobotGeometries { - // fmt.Println("g.Pose: ", spatialmath.PoseToProtobuf(g.Pose())) - // } // find all geometries that are not moving but are in the frame system staticRobotGeometries := make([]spatialmath.Geometry, 0) @@ -527,10 +523,6 @@ func (pm *planManager) plannerSetupFromMoveRequest( if err != nil { return nil, err } - fmt.Println("ABOUT TO PRINT WORLD GEOMETRIES ") - for _, g := range worldGeometries.Geometries() { - fmt.Println("g.Pose: ", spatialmath.PoseToProtobuf(g.Pose())) - } allowedCollisions, err := collisionSpecificationsFromProto(constraints.GetCollisionSpecification(), frameSystemGeometries, worldState) if err != nil { diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index bd370c37344..d3ac31dcbf5 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -288,12 +288,18 @@ func TestPtgCheckPlan(t *testing.T) { // NOTE: WE NEED TO ADD AN EXECUTION FRAME TO THE CHECKING FRAMESYSTEM SINCE WE ONLY WANT TO RELY ON USING INPUTS tfFrameSystem := referenceframe.NewEmptyFrameSystem("transformFS") - localizationFrame, err := referenceframe.New2DMobileModelFrame( - "ackframeLocalizationFrame", []referenceframe.Limit{ + localizationFrame, err := referenceframe.New7DFrame( + "ackframeLocalizationFrame", + []referenceframe.Limit{ {Min: math.Inf(-1), Max: math.Inf(1)}, {Min: math.Inf(-1), Max: math.Inf(1)}, - {Min: -360, Max: 360}, - }, nil, + {Min: math.Inf(-1), Max: math.Inf(1)}, + {Min: -1, Max: 1}, + {Min: -1, Max: 1}, + {Min: -1, Max: 1}, + {Min: -2 * math.Pi, Max: 2 * math.Pi}, + }, + nil, ) test.That(t, err, test.ShouldBeNil) @@ -302,7 +308,7 @@ func TestPtgCheckPlan(t *testing.T) { err = tfFrameSystem.MergeFrameSystem(fs, localizationFrame) test.That(t, err, test.ShouldBeNil) - inputs[localizationFrame.Name()] = referenceframe.FloatsToInputs(make([]float64, len(localizationFrame.DoF()))) + inputs[localizationFrame.Name()] = referenceframe.FloatsToInputs([]float64{0, 0, 0, 0, 0, 1, 0}) t.Run("base case - validate plan without obstacles", func(t *testing.T) { executionState := ExecutionState{ diff --git a/referenceframe/frame.go b/referenceframe/frame.go index cbfa391aa2d..4539fae0a1a 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -451,7 +451,7 @@ type sevenDFrame struct { collisionGeometry spatial.Geometry } -// New6DFrame TODO. +// New7DFrame created a frame with 7 degrees of freedom. func New7DFrame(name string, limits []Limit, collisionGeometry spatial.Geometry) (Frame, error) { if len(limits) != 7 { return nil, @@ -462,7 +462,6 @@ func New7DFrame(name string, limits []Limit, collisionGeometry spatial.Geometry) &baseFrame{name, limits}, collisionGeometry, }, nil - } // Transform takes a model and a list of joint angles in radians and computes the dual quaternion representing the diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 0f75dff259c..1776a7506c0 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -334,53 +334,53 @@ func TestObstacleReplanningGlobe(t *testing.T) { } extra := map[string]interface{}{"max_replans": 10, "max_ik_solutions": 1, "smooth_iter": 1} - // extraNoReplan := map[string]interface{}{"max_replans": 0, "max_ik_solutions": 1, "smooth_iter": 1} + extraNoReplan := map[string]interface{}{"max_replans": 0, "max_ik_solutions": 1, "smooth_iter": 1} // We set a flag here per test case so that detections are not returned the first time each vision service is called testCases := []testCase{ - // { - // name: "ensure no replan from discovered obstacles", - // getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { - // caseName := "test-case-1" - // obstaclePosition := spatialmath.NewPoseFromPoint(r3.Vector{X: -1000, Y: -1000, Z: 0}) - // box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 10, Y: 10, Z: 10}, caseName) - // test.That(t, err, test.ShouldBeNil) - - // detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) - // test.That(t, err, test.ShouldBeNil) - - // return []*viz.Object{detection}, nil - // }, - // expectedSuccess: true, - // extra: extraNoReplan, - // }, - // { - // name: "ensure replan due to obstacle collision", - // getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { - // caseName := "test-case-2" - // // The camera is parented to the base. Thus, this will always see an obstacle 300mm in front of where the base is. - // // Note: for createMoveOnGlobeEnvironment, 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) - // test.That(t, err, test.ShouldBeNil) - - // detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) - // test.That(t, err, test.ShouldBeNil) - - // return []*viz.Object{detection}, nil - // }, - // expectedSuccess: false, - // expectedErr: fmt.Sprintf("exceeded maximum number of replans: %d: plan failed", 0), - // extra: extraNoReplan, - // }, + { + name: "ensure no replan from discovered obstacles", + getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { + caseName := "test-case-1" + obstaclePosition := spatialmath.NewPoseFromPoint(r3.Vector{X: -1000, Y: -1000, Z: 0}) + box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 10, Y: 10, Z: 10}, caseName) + test.That(t, err, test.ShouldBeNil) + + detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) + test.That(t, err, test.ShouldBeNil) + + return []*viz.Object{detection}, nil + }, + expectedSuccess: true, + extra: extraNoReplan, + }, + { + name: "ensure replan due to obstacle collision", + getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { + caseName := "test-case-2" + // The camera is parented to the base. Thus, this will always see an obstacle 300mm in front of where the base is. + // Note: for createMoveOnGlobeEnvironment, 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) + test.That(t, err, test.ShouldBeNil) + + detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) + test.That(t, err, test.ShouldBeNil) + + return []*viz.Object{detection}, nil + }, + expectedSuccess: false, + expectedErr: fmt.Sprintf("exceeded maximum number of replans: %d: plan failed", 0), + extra: extraNoReplan, + }, { 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. // However, enough replans should eventually get it to its goal. - obstaclePosition := spatialmath.NewPoseFromPoint(r3.Vector{X: 800, Y: 0, Z: 0}) + obstaclePosition := spatialmath.NewPoseFromPoint(r3.Vector{X: 500, Y: 0, Z: 0}) box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 20, Y: 20, Z: 10}, caseName) test.That(t, err, test.ShouldBeNil) diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 54541f8da58..fd5fba8c849 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -184,29 +184,12 @@ func (mr *moveRequest) getTransientDetections( if err != nil { return nil, err } - inputMap := referenceframe.StartPositions(mr.absoluteFS) - - fmt.Println("baseExecutionState.CurrentInputs(): ", baseExecutionState.CurrentInputs()) - fmt.Println("baseExecutionState.CurrentPoses(): ", baseExecutionState.CurrentPoses()) - - inputMap[mr.kinematicBase.Name().ShortName()] = make([]referenceframe.Input, len(mr.kinematicBase.Kinematics().DoF())) - inputMap[mr.kinematicBase.LocalizationFrame().Name()] = referenceframe.FloatsToInputs([]float64{ - baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().X, - baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().Y, - baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().Z, - baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().OX, - baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().OY, - baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().OZ, - baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().Theta, - }) - // fmt.Println("do i get to this point in time though") + inputMap := mr.createInputMap(baseExecutionState) detections, err := visSrvc.GetObjectPointClouds(ctx, camName.Name, nil) if err != nil { return nil, err } - fmt.Println("inputMap: ", inputMap) - // fmt.Println("mr.kinematicBase.LocalizationFrame().DoF(): ", mr.kinematicBase.LocalizationFrame().DoF()) // transformed detections transientGeoms := []spatialmath.Geometry{} @@ -217,7 +200,6 @@ func (mr *moveRequest) getTransientDetections( if geometry.Label() != "" { label += "_" + geometry.Label() } - fmt.Println("current geometry pose: ", spatialmath.PoseToProtobuf(geometry.Pose())) geometry.SetLabel(label) tf, err := mr.absoluteFS.Transform( inputMap, @@ -231,12 +213,6 @@ func (mr *moveRequest) getTransientDetections( if !ok { return nil, errors.New("unable to assert referenceframe.Transformable into *referenceframe.GeometriesInFrame") } - fmt.Println("ABOUT TO PRINT WORLD GIF GEOMETRIES") - for _, g := range worldGifs.Geometries() { - fmt.Println("g.Pose: ", spatialmath.PoseToProtobuf(g.Pose())) - - } - fmt.Println(" ") transientGeoms = append(transientGeoms, worldGifs.Geometries()...) } return referenceframe.NewGeometriesInFrame(referenceframe.World, transientGeoms), nil @@ -270,7 +246,6 @@ func (mr *moveRequest) obstaclesIntersectPlan( if err != nil { return state.ExecuteResponse{}, err } - // fmt.Println("do i make it here though?") if len(gifs.Geometries()) == 0 { mr.logger.CDebug(ctx, "will not check if obstacles intersect path since nothing was detected") continue @@ -291,18 +266,7 @@ 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.absoluteFS) - // inputMap[mr.kinematicBase.Name().ShortName()] = baseExecutionState.CurrentInputs()[mr.kinematicBase.Name().ShortName()] - inputMap[mr.kinematicBase.Name().ShortName()] = make([]referenceframe.Input, len(mr.kinematicBase.Kinematics().DoF())) - inputMap[mr.kinematicBase.LocalizationFrame().Name()] = referenceframe.FloatsToInputs([]float64{ - baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().X, - baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().Y, - baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().Z, - baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().OX, - baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().OY, - baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().OZ, - baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().Theta, - }) + inputMap := mr.createInputMap(baseExecutionState) executionState, err := motionplan.NewExecutionState( baseExecutionState.Plan(), baseExecutionState.Index(), @@ -312,7 +276,7 @@ func (mr *moveRequest) obstaclesIntersectPlan( if err != nil { return state.ExecuteResponse{}, err } - // fmt.Println("PRINTING CHECK PLAN INPUTS BELOW") + mr.logger.CDebugf(ctx, "CheckPlan inputs: \n currentPosition: %v\n currentInputs: %v\n worldstate: %s", spatialmath.PoseToProtobuf(executionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose()), inputMap, @@ -327,18 +291,29 @@ func (mr *moveRequest) obstaclesIntersectPlan( lookAheadDistanceMM, mr.planRequest.Logger, ); err != nil { - fmt.Println("WE ARE RETURNING AN ERROR") - fmt.Println(" ") mr.planRequest.Logger.CInfo(ctx, err.Error()) return state.ExecuteResponse{Replan: true, ReplanReason: err.Error()}, nil } - fmt.Println("THERE WAS NO ERROR HERE") - fmt.Println(" ") } } return state.ExecuteResponse{}, nil } +func (mr *moveRequest) createInputMap(baseExecutionState motionplan.ExecutionState) map[string][]referenceframe.Input { + inputMap := referenceframe.StartPositions(mr.absoluteFS) + inputMap[mr.kinematicBase.Name().ShortName()] = make([]referenceframe.Input, len(mr.kinematicBase.Kinematics().DoF())) + inputMap[mr.kinematicBase.LocalizationFrame().Name()] = referenceframe.FloatsToInputs([]float64{ + baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().X, + baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().Y, + baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().Z, + baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().OX, + baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().OY, + baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().OZ, + baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().Theta, + }) + return inputMap +} + func kbOptionsFromCfg(motionCfg *validatedMotionConfiguration, validatedExtra validatedExtra) kinematicbase.Options { kinematicsOptions := kinematicbase.NewKinematicBaseOptions() From a3e1ee2076494e8546acd69c5e28cf2d88b4856a Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 24 May 2024 11:25:09 -0400 Subject: [PATCH 056/126] over-write inputs to correct first place frame but have original info intact to interpolate properly --- motionplan/check.go | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/motionplan/check.go b/motionplan/check.go index 30b9739dff4..4de2a3d9299 100644 --- a/motionplan/check.go +++ b/motionplan/check.go @@ -188,11 +188,14 @@ func checkPlanRelative( return err } + overWrittenInputs := startingInputs + overWrittenInputs[checkFrame.Name()] = make([]referenceframe.Input, len(checkFrame.DoF())) + // 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, + overWrittenInputs, worldState, nil, // no pb.Constraints nil, // no plannOpts From 6a3a79486bf388359928091973832cc669095705 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 24 May 2024 11:25:56 -0400 Subject: [PATCH 057/126] overwrite inputs for placing detection properly --- services/motion/builtin/move_request.go | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index fd5fba8c849..d413f2a966b 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -185,6 +185,7 @@ func (mr *moveRequest) getTransientDetections( return nil, err } inputMap := mr.createInputMap(baseExecutionState) + inputMap[mr.kinematicBase.Name().ShortName()] = make([]referenceframe.Input, len(mr.kinematicBase.Kinematics().DoF())) detections, err := visSrvc.GetObjectPointClouds(ctx, camName.Name, nil) if err != nil { @@ -301,7 +302,7 @@ func (mr *moveRequest) obstaclesIntersectPlan( func (mr *moveRequest) createInputMap(baseExecutionState motionplan.ExecutionState) map[string][]referenceframe.Input { inputMap := referenceframe.StartPositions(mr.absoluteFS) - inputMap[mr.kinematicBase.Name().ShortName()] = make([]referenceframe.Input, len(mr.kinematicBase.Kinematics().DoF())) + inputMap[mr.kinematicBase.Name().ShortName()] = baseExecutionState.CurrentInputs()[mr.kinematicBase.Name().Name] inputMap[mr.kinematicBase.LocalizationFrame().Name()] = referenceframe.FloatsToInputs([]float64{ baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().X, baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().Y, From 668a27a5e4a1094be76a7ae4c235acfdc602ab27 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Tue, 28 May 2024 10:31:25 -0400 Subject: [PATCH 058/126] rays comments sans interpolateInputs requested change --- .../base/kinematicbase/fake_kinematics.go | 2 +- .../base/kinematicbase/ptgKinematics.go | 2 +- motionplan/tpSpaceRRT_test.go | 2 +- referenceframe/frame.go | 73 +++++++++--------- services/motion/builtin/builtin_test.go | 75 +++++++++---------- 5 files changed, 79 insertions(+), 75 deletions(-) diff --git a/components/base/kinematicbase/fake_kinematics.go b/components/base/kinematicbase/fake_kinematics.go index ca1671a2f4b..e6bff05ff43 100644 --- a/components/base/kinematicbase/fake_kinematics.go +++ b/components/base/kinematicbase/fake_kinematics.go @@ -200,7 +200,7 @@ func WrapWithFakePTGKinematics( } // construct localization frame - localizationFrame, err := referenceframe.New7DFrame( + localizationFrame, err := referenceframe.NewPoseFrame( b.Name().ShortName()+"LocalizationFrame", []referenceframe.Limit{ {Min: math.Inf(-1), Max: math.Inf(1)}, diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index ef7c0810c81..966b185ff7e 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -141,7 +141,7 @@ func wrapWithPTGKinematics( } startingState := baseState{currentInputs: zeroInput} // construct localization frame - localizationFrame, err := referenceframe.New7DFrame( + localizationFrame, err := referenceframe.NewPoseFrame( b.Name().ShortName()+"LocalizationFrame", []referenceframe.Limit{ {Min: math.Inf(-1), Max: math.Inf(1)}, diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index d3ac31dcbf5..4d5f307d44a 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -288,7 +288,7 @@ func TestPtgCheckPlan(t *testing.T) { // NOTE: WE NEED TO ADD AN EXECUTION FRAME TO THE CHECKING FRAMESYSTEM SINCE WE ONLY WANT TO RELY ON USING INPUTS tfFrameSystem := referenceframe.NewEmptyFrameSystem("transformFS") - localizationFrame, err := referenceframe.New7DFrame( + localizationFrame, err := referenceframe.NewPoseFrame( "ackframeLocalizationFrame", []referenceframe.Limit{ {Min: math.Inf(-1), Max: math.Inf(1)}, diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 4539fae0a1a..a90fcfb4ffb 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -446,28 +446,28 @@ func (rf rotationalFrame) MarshalJSON() ([]byte, error) { return json.Marshal(temp) } -type sevenDFrame struct { +type poseFrame struct { *baseFrame - collisionGeometry spatial.Geometry + geometry spatial.Geometry } -// New7DFrame created a frame with 7 degrees of freedom. -func New7DFrame(name string, limits []Limit, collisionGeometry spatial.Geometry) (Frame, error) { +// NewPoseFrame created a frame with 7 degrees of freedom, x, y, z, OX, OY, OZ, and theta in radians. +func NewPoseFrame(name string, limits []Limit, geometry spatial.Geometry) (Frame, error) { if len(limits) != 7 { return nil, errors.Errorf("Must have 7DOF state (x, y, z, OX, OY, OZ, theta) to create 6DFrame, have %d dof", len(limits)) } - return &sevenDFrame{ + return &poseFrame{ &baseFrame{name, limits}, - collisionGeometry, + geometry, }, nil } -// Transform takes a model and a list of joint angles in radians and computes the dual quaternion representing the -// cartesian position of the end effector. This is useful for when conversions between quaternions and OV are not needed. -func (m *sevenDFrame) Transform(inputs []Input) (spatial.Pose, error) { - if len(inputs) != 7 { +// Transform on the pose frame 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, NewIncorrectInputLengthError(len(inputs), 7) } return spatial.NewPose( @@ -482,42 +482,43 @@ func (m *sevenDFrame) Transform(inputs []Input) (spatial.Pose, error) { } // Interpolate interpolates the given amount between the two sets of inputs. -func (m *sevenDFrame) Interpolate(from, to []Input, by float64) ([]Input, error) { - if len(from) != len(m.DoF()) { - return nil, NewIncorrectInputLengthError(len(from), len(m.DoF())) +func (pf *poseFrame) Interpolate(from, to []Input, by float64) ([]Input, error) { + if err := pf.baseFrame.validInputs(from); err != nil { + return nil, NewIncorrectInputLengthError(len(from), 7) } - if len(to) != len(m.DoF()) { - return nil, NewIncorrectInputLengthError(len(to), len(m.DoF())) + if err := pf.baseFrame.validInputs(to); err != nil { + return nil, NewIncorrectInputLengthError(len(to), 7) } + // todo: these inputs need to be interpolated correctly return interpolateInputs(from, to, by), nil } // Geometries returns an object representing the 3D space associeted with the staticFrame. -func (m *sevenDFrame) Geometries(inputs []Input) (*GeometriesInFrame, error) { - transformByPose, err := m.Transform(inputs) +func (pf *poseFrame) Geometries(inputs []Input) (*GeometriesInFrame, error) { + transformByPose, err := pf.Transform(inputs) if err != nil { return nil, err } - if m.collisionGeometry == nil { - return NewGeometriesInFrame(m.name, []spatial.Geometry{}), nil + if pf.geometry == nil { + return NewGeometriesInFrame(pf.name, []spatial.Geometry{}), nil } - return NewGeometriesInFrame(m.name, []spatial.Geometry{m.collisionGeometry.Transform(transformByPose)}), nil + return NewGeometriesInFrame(pf.name, []spatial.Geometry{pf.geometry.Transform(transformByPose)}), nil } // DoF returns the number of degrees of freedom within a model. -func (m *sevenDFrame) DoF() []Limit { - return m.limits +func (pf *poseFrame) DoF() []Limit { + return pf.limits } // MarshalJSON serializes a Model. -func (m *sevenDFrame) MarshalJSON() ([]byte, error) { +func (pf *poseFrame) MarshalJSON() ([]byte, error) { temp := LinkConfig{ - ID: m.name, + ID: pf.name, } - if m.collisionGeometry != nil { + if pf.geometry != nil { var err error - temp.Geometry, err = spatial.NewGeometryConfig(m.collisionGeometry) + temp.Geometry, err = spatial.NewGeometryConfig(pf.geometry) if err != nil { return nil, err } @@ -526,15 +527,19 @@ func (m *sevenDFrame) MarshalJSON() ([]byte, error) { } // InputFromProtobuf converts pb.JointPosition to inputs. -func (m *sevenDFrame) InputFromProtobuf(jp *pb.JointPositions) []Input { - inputs := make([]Input, 0, len(jp.Values)) - - return inputs +func (pf *poseFrame) InputFromProtobuf(jp *pb.JointPositions) []Input { + n := make([]Input, len(jp.Values)) + for idx, d := range jp.Values { + n[idx] = Input{utils.DegToRad(d)} + } + return n } // ProtobufFromInput converts inputs to pb.JointPosition. -func (m *sevenDFrame) ProtobufFromInput(input []Input) *pb.JointPositions { - jPos := &pb.JointPositions{} - - return jPos +func (pf *poseFrame) ProtobufFromInput(input []Input) *pb.JointPositions { + n := make([]float64, len(input)) + for idx, a := range input { + n[idx] = utils.RadToDeg(a.Value) + } + return &pb.JointPositions{Values: n} } diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 1776a7506c0..586c15e07a3 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -334,51 +334,50 @@ func TestObstacleReplanningGlobe(t *testing.T) { } extra := map[string]interface{}{"max_replans": 10, "max_ik_solutions": 1, "smooth_iter": 1} - extraNoReplan := map[string]interface{}{"max_replans": 0, "max_ik_solutions": 1, "smooth_iter": 1} + // extraNoReplan := map[string]interface{}{"max_replans": 0, "max_ik_solutions": 1, "smooth_iter": 1} // We set a flag here per test case so that detections are not returned the first time each vision service is called testCases := []testCase{ - { - name: "ensure no replan from discovered obstacles", - getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { - caseName := "test-case-1" - obstaclePosition := spatialmath.NewPoseFromPoint(r3.Vector{X: -1000, Y: -1000, Z: 0}) - box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 10, Y: 10, Z: 10}, caseName) - test.That(t, err, test.ShouldBeNil) - - detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) - test.That(t, err, test.ShouldBeNil) - - return []*viz.Object{detection}, nil - }, - expectedSuccess: true, - extra: extraNoReplan, - }, - { - name: "ensure replan due to obstacle collision", - getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { - caseName := "test-case-2" - // The camera is parented to the base. Thus, this will always see an obstacle 300mm in front of where the base is. - // Note: for createMoveOnGlobeEnvironment, 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) - test.That(t, err, test.ShouldBeNil) - - detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) - test.That(t, err, test.ShouldBeNil) - - return []*viz.Object{detection}, nil - }, - expectedSuccess: false, - expectedErr: fmt.Sprintf("exceeded maximum number of replans: %d: plan failed", 0), - extra: extraNoReplan, - }, + // { + // name: "ensure no replan from discovered obstacles", + // getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { + // caseName := "test-case-1" + // obstaclePosition := spatialmath.NewPoseFromPoint(r3.Vector{X: -1000, Y: -1000, Z: 0}) + // box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 10, Y: 10, Z: 10}, caseName) + // test.That(t, err, test.ShouldBeNil) + + // detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) + // test.That(t, err, test.ShouldBeNil) + + // return []*viz.Object{detection}, nil + // }, + // expectedSuccess: true, + // extra: extraNoReplan, + // }, + // { + // name: "ensure replan due to obstacle collision", + // getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { + // caseName := "test-case-2" + // // The camera is parented to the base. Thus, this will always see an obstacle 300mm in front of where the base is. + // // Note: for createMoveOnGlobeEnvironment, 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) + // test.That(t, err, test.ShouldBeNil) + + // detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) + // test.That(t, err, test.ShouldBeNil) + // return []*viz.Object{detection}, nil + // }, + // expectedSuccess: false, + // expectedErr: fmt.Sprintf("exceeded maximum number of replans: %d: plan failed", 0), + // extra: extraNoReplan, + // }, { 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 500mm in front of it, triggering several replans. // However, enough replans should eventually get it to its goal. obstaclePosition := spatialmath.NewPoseFromPoint(r3.Vector{X: 500, Y: 0, Z: 0}) box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 20, Y: 20, Z: 10}, caseName) From 7648907ef49509e2670b99bba6f4473bfd1c0dc1 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Tue, 28 May 2024 11:36:21 -0400 Subject: [PATCH 059/126] hoppefully fixed TestObstacleReplanningGlobe --- .../base/kinematicbase/fake_kinematics.go | 6 +- .../base/kinematicbase/ptgKinematics.go | 6 +- motionplan/tpSpaceRRT_test.go | 6 +- referenceframe/frame.go | 3 +- services/motion/builtin/builtin_test.go | 74 +++++++++---------- 5 files changed, 47 insertions(+), 48 deletions(-) diff --git a/components/base/kinematicbase/fake_kinematics.go b/components/base/kinematicbase/fake_kinematics.go index e6bff05ff43..1a95196cf14 100644 --- a/components/base/kinematicbase/fake_kinematics.go +++ b/components/base/kinematicbase/fake_kinematics.go @@ -206,9 +206,9 @@ func WrapWithFakePTGKinematics( {Min: math.Inf(-1), Max: math.Inf(1)}, {Min: math.Inf(-1), Max: math.Inf(1)}, {Min: math.Inf(-1), Max: math.Inf(1)}, - {Min: -1, Max: 1}, - {Min: -1, Max: 1}, - {Min: -1, Max: 1}, + {Min: -1.5, Max: 1.5}, + {Min: -1.5, Max: 1.5}, + {Min: -1.5, Max: 1.5}, {Min: -2 * math.Pi, Max: 2 * math.Pi}, }, nil, diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index 966b185ff7e..cb00492ee1a 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -147,9 +147,9 @@ func wrapWithPTGKinematics( {Min: math.Inf(-1), Max: math.Inf(1)}, {Min: math.Inf(-1), Max: math.Inf(1)}, {Min: math.Inf(-1), Max: math.Inf(1)}, - {Min: -1, Max: 1}, - {Min: -1, Max: 1}, - {Min: -1, Max: 1}, + {Min: -1.5, Max: 1.5}, + {Min: -1.5, Max: 1.5}, + {Min: -1.5, Max: 1.5}, {Min: -2 * math.Pi, Max: 2 * math.Pi}, }, nil, diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index 4d5f307d44a..ea47f62394d 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -294,9 +294,9 @@ func TestPtgCheckPlan(t *testing.T) { {Min: math.Inf(-1), Max: math.Inf(1)}, {Min: math.Inf(-1), Max: math.Inf(1)}, {Min: math.Inf(-1), Max: math.Inf(1)}, - {Min: -1, Max: 1}, - {Min: -1, Max: 1}, - {Min: -1, Max: 1}, + {Min: -1.5, Max: 1.5}, + {Min: -1.5, Max: 1.5}, + {Min: -1.5, Max: 1.5}, {Min: -2 * math.Pi, Max: 2 * math.Pi}, }, nil, diff --git a/referenceframe/frame.go b/referenceframe/frame.go index a90fcfb4ffb..37bd9be0c2e 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -489,8 +489,7 @@ func (pf *poseFrame) Interpolate(from, to []Input, by float64) ([]Input, error) if err := pf.baseFrame.validInputs(to); err != nil { return nil, NewIncorrectInputLengthError(len(to), 7) } - // todo: these inputs need to be interpolated correctly - return interpolateInputs(from, to, by), nil + return from, nil } // Geometries returns an object representing the 3D space associeted with the staticFrame. diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 586c15e07a3..39f6f4a8715 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -334,52 +334,52 @@ func TestObstacleReplanningGlobe(t *testing.T) { } extra := map[string]interface{}{"max_replans": 10, "max_ik_solutions": 1, "smooth_iter": 1} - // extraNoReplan := map[string]interface{}{"max_replans": 0, "max_ik_solutions": 1, "smooth_iter": 1} + extraNoReplan := map[string]interface{}{"max_replans": 0, "max_ik_solutions": 1, "smooth_iter": 1} // We set a flag here per test case so that detections are not returned the first time each vision service is called testCases := []testCase{ - // { - // name: "ensure no replan from discovered obstacles", - // getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { - // caseName := "test-case-1" - // obstaclePosition := spatialmath.NewPoseFromPoint(r3.Vector{X: -1000, Y: -1000, Z: 0}) - // box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 10, Y: 10, Z: 10}, caseName) - // test.That(t, err, test.ShouldBeNil) - - // detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) - // test.That(t, err, test.ShouldBeNil) - - // return []*viz.Object{detection}, nil - // }, - // expectedSuccess: true, - // extra: extraNoReplan, - // }, - // { - // name: "ensure replan due to obstacle collision", - // getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { - // caseName := "test-case-2" - // // The camera is parented to the base. Thus, this will always see an obstacle 300mm in front of where the base is. - // // Note: for createMoveOnGlobeEnvironment, 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) - // test.That(t, err, test.ShouldBeNil) - - // detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) - // test.That(t, err, test.ShouldBeNil) - // return []*viz.Object{detection}, nil - // }, - // expectedSuccess: false, - // expectedErr: fmt.Sprintf("exceeded maximum number of replans: %d: plan failed", 0), - // extra: extraNoReplan, - // }, + { + name: "ensure no replan from discovered obstacles", + getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { + caseName := "test-case-1" + obstaclePosition := spatialmath.NewPoseFromPoint(r3.Vector{X: -1000, Y: -1000, Z: 0}) + box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 10, Y: 10, Z: 10}, caseName) + test.That(t, err, test.ShouldBeNil) + + detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) + test.That(t, err, test.ShouldBeNil) + + return []*viz.Object{detection}, nil + }, + expectedSuccess: true, + extra: extraNoReplan, + }, + { + name: "ensure replan due to obstacle collision", + getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { + caseName := "test-case-2" + // The camera is parented to the base. Thus, this will always see an obstacle 300mm in front of where the base is. + // Note: for createMoveOnGlobeEnvironment, 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) + test.That(t, err, test.ShouldBeNil) + + detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) + test.That(t, err, test.ShouldBeNil) + return []*viz.Object{detection}, nil + }, + expectedSuccess: false, + expectedErr: fmt.Sprintf("exceeded maximum number of replans: %d: plan failed", 0), + extra: extraNoReplan, + }, { 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 500mm in front of it, triggering several replans. // However, enough replans should eventually get it to its goal. - obstaclePosition := spatialmath.NewPoseFromPoint(r3.Vector{X: 500, Y: 0, Z: 0}) + obstaclePosition := spatialmath.NewPoseFromPoint(r3.Vector{X: 800, Y: 0, Z: 0}) box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 20, Y: 20, Z: 10}, caseName) test.That(t, err, test.ShouldBeNil) From 5004e4c2b095a4cc6c3c4d3edaed1e7d0b9a64ab Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Tue, 28 May 2024 12:07:51 -0400 Subject: [PATCH 060/126] fix TestPtgCheckPlan --- motionplan/tpSpaceRRT_test.go | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index ea47f62394d..0da7fa66df5 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -461,7 +461,7 @@ func TestPtgCheckPlan(t *testing.T) { test.That(t, err, test.ShouldBeNil) pathPose := remainingPlan.Path()[0][ackermanFrame.Name()].Pose() - startPose := spatialmath.NewPose(r3.Vector{0, 1000, 0}, pathPose.Orientation()) + startPose := spatialmath.NewPose(r3.Vector{1000, 0, 0}, pathPose.Orientation()) executionState := ExecutionState{ plan: plan, From 3f2df509f6d74972b2345a63bf471b2edbd49b2f Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 5 Jun 2024 16:28:28 -0400 Subject: [PATCH 061/126] move anyNonZero check from solverFrame into planManager, as it was originally there --- motionplan/planManager.go | 12 ++++++++++++ motionplan/solverFrame.go | 9 +-------- 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/motionplan/planManager.go b/motionplan/planManager.go index 3484a8fd5d0..58d9626e1cf 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -742,6 +742,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") } diff --git a/motionplan/solverFrame.go b/motionplan/solverFrame.go index 1c362a6df88..0821b2ff338 100644 --- a/motionplan/solverFrame.go +++ b/motionplan/solverFrame.go @@ -150,8 +150,6 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, var ptgs []tpspace.PTGSolver anyPTG := false // Whether PTG frames have been observed - // TODO(nf): uncomment - // anyNonzero := false // Whether non-PTG frames for _, movingFrame := range frames { if ptgFrame, isPTGframe := movingFrame.(tpspace.PTGProvider); isPTGframe { if anyPTG { @@ -159,12 +157,7 @@ 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") - // } + } } return &solverFrame{ From ec18e952d51328fc89cebac7723d3e0baa083d81 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 5 Jun 2024 16:38:45 -0400 Subject: [PATCH 062/126] add localizationFrame as argument to CheckPlan + update tests --- .../base/kinematicbase/differentialDrive.go | 2 +- .../base/kinematicbase/ptgKinematics.go | 1 - motionplan/check.go | 20 ++++++++++--------- motionplan/constraint.go | 11 ---------- services/motion/builtin/move_on_map_test.go | 1 + services/motion/builtin/move_request.go | 1 + services/motion/explore/explore.go | 1 + 7 files changed, 15 insertions(+), 22 deletions(-) diff --git a/components/base/kinematicbase/differentialDrive.go b/components/base/kinematicbase/differentialDrive.go index 5ad84048e64..cf1a9247ca9 100644 --- a/components/base/kinematicbase/differentialDrive.go +++ b/components/base/kinematicbase/differentialDrive.go @@ -100,7 +100,7 @@ type differentialDriveKinematics struct { base.Base motion.Localizer logger logging.Logger - planningFrame, localizationFrame referenceframe.Model + planningFrame, localizationFrame referenceframe.Frame options Options noLocalizerCacheInputs []referenceframe.Input currentTrajectory [][]referenceframe.Input diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index cb00492ee1a..b695668acc5 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -192,7 +192,6 @@ func (ptgk *ptgBaseKinematics) CurrentInputs(ctx context.Context) ([]referencefr defer ptgk.inputLock.RUnlock() planningFrameInputs := ptgk.currentState.currentInputs - planningFrameInputs = append(planningFrameInputs, []referenceframe.Input{}...) return planningFrameInputs, nil } diff --git a/motionplan/check.go b/motionplan/check.go index 4de2a3d9299..58e7080b5c7 100644 --- a/motionplan/check.go +++ b/motionplan/check.go @@ -18,7 +18,7 @@ import ( // detected, the interpolated position of the rover when a collision is detected is returned along // with an error with additional collision details. func CheckPlan( - checkFrame referenceframe.Frame, // TODO(RSDK-7421): remove this + checkFrame, localizationFrame referenceframe.Frame, // TODO(RSDK-7421): remove this executionState ExecutionState, worldState *referenceframe.WorldState, fs referenceframe.FrameSystem, @@ -53,13 +53,16 @@ func CheckPlan( // Currently this is only TP-space, so we check if the PTG length is >0. // The solver frame will have had its PTGs filled in the newPlanManager() call, if applicable. if sfPlanner.useTPspace { - return checkPlanRelative(checkFrame, executionState, worldState, fs, lookAheadDistanceMM, sfPlanner) + if localizationFrame == nil { + return errors.New("cannot have nil localizationFrame when checking PTG plans") + } + return checkPlanRelative(checkFrame, localizationFrame, executionState, worldState, fs, lookAheadDistanceMM, sfPlanner) } return checkPlanAbsolute(checkFrame, executionState, worldState, fs, lookAheadDistanceMM, sfPlanner) } func checkPlanRelative( - checkFrame referenceframe.Frame, // TODO(RSDK-7421): remove this + checkFrame, localizationFrame referenceframe.Frame, // TODO(RSDK-7421): remove this executionState ExecutionState, worldState *referenceframe.WorldState, fs referenceframe.FrameSystem, @@ -125,10 +128,9 @@ func checkPlanRelative( if currentPoses == nil { return errors.New("executionState had nil return from CurrentPoses") } - localizationFrameName := checkFrame.Name() + "LocalizationFrame" - currentPoseIF, ok := currentPoses[localizationFrameName] + currentPoseIF, ok := currentPoses[localizationFrame.Name()] if !ok { - return errors.New("checkFrameLocalizationFrame not found in current pose map") + return errors.New("LocalizationFrame not found in current pose map") } sf := sfPlanner.frame @@ -216,7 +218,7 @@ func checkPlanRelative( return err } currentWayPointTraj := plan.Trajectory()[wayPointIdx] - currentWayPointTraj[checkFrame.Name()+"LocalizationFrame"] = referenceframe.FloatsToInputs([]float64{ + currentWayPointTraj[localizationFrame.Name()] = referenceframe.FloatsToInputs([]float64{ poses[wayPointIdx].Point().X, poses[wayPointIdx].Point().Y, poses[wayPointIdx].Point().Z, @@ -265,7 +267,7 @@ func checkPlanRelative( } } - startInputs[localizationFrameName] = referenceframe.FloatsToInputs( + startInputs[localizationFrame.Name()] = referenceframe.FloatsToInputs( []float64{ lastArcEndPose.Point().X, lastArcEndPose.Point().Y, lastArcEndPose.Point().Z, lastArcEndPose.Orientation().OrientationVectorRadians().OX, @@ -275,7 +277,7 @@ func checkPlanRelative( }, ) nextInputs := plan.Trajectory()[i] - nextInputs[localizationFrameName] = startInputs[localizationFrameName] + nextInputs[localizationFrame.Name()] = startInputs[localizationFrame.Name()] segment, err := createSegment(sf, lastArcEndPose, thisArcEndPose, startInputs, nextInputs) if err != nil { return err diff --git a/motionplan/constraint.go b/motionplan/constraint.go index 786afdd0f6b..14d527c99cb 100644 --- a/motionplan/constraint.go +++ b/motionplan/constraint.go @@ -335,17 +335,6 @@ func NewCollisionConstraint( return false } internalGeoms = internal.Geometries() - case state.Position != nil: - // If we didn't pass a Configuration, but we do have a Position, then get the geometries at the zero state and - // transform them to the Position - internal, err := state.Frame.Geometries(make([]referenceframe.Input, len(state.Frame.DoF()))) - if err != nil { - return false - } - movedGeoms := internal.Geometries() - for _, geom := range movedGeoms { - internalGeoms = append(internalGeoms, geom.Transform(state.Position)) - } default: return false } diff --git a/services/motion/builtin/move_on_map_test.go b/services/motion/builtin/move_on_map_test.go index 66e523d21bb..d7032e8293c 100644 --- a/services/motion/builtin/move_on_map_test.go +++ b/services/motion/builtin/move_on_map_test.go @@ -607,6 +607,7 @@ func TestMoveOnMapStaticObs(t *testing.T) { test.That(t, err, test.ShouldBeNil) err = motionplan.CheckPlan( mr.planRequest.Frame, + mr.kinematicBase.LocalizationFrame(), executionState, wrldSt, mr.absoluteFS, diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index d413f2a966b..f24344b7725 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -286,6 +286,7 @@ func (mr *moveRequest) obstaclesIntersectPlan( if err := motionplan.CheckPlan( mr.kinematicBase.Kinematics(), // frame we wish to check for collisions + mr.kinematicBase.LocalizationFrame(), executionState, worldState, // detected obstacles by this instance of camera + service mr.absoluteFS, diff --git a/services/motion/explore/explore.go b/services/motion/explore/explore.go index f5dacc5ebe0..777843e8c8b 100644 --- a/services/motion/explore/explore.go +++ b/services/motion/explore/explore.go @@ -379,6 +379,7 @@ func (ms *explore) checkForObstacles( // Check plan for transient obstacles err = motionplan.CheckPlan( kb.Kinematics(), + nil, executionState, worldState, ms.frameSystem, From 3e186d9a4a411c3283d14d384edbfedb39881f82 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 5 Jun 2024 16:46:30 -0400 Subject: [PATCH 063/126] add missing arguments to test files --- motionplan/motionPlanner_test.go | 4 ++-- motionplan/tpSpaceRRT_test.go | 12 ++++++------ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/motionplan/motionPlanner_test.go b/motionplan/motionPlanner_test.go index 1072b347f71..df9a01e0ecb 100644 --- a/motionplan/motionPlanner_test.go +++ b/motionplan/motionPlanner_test.go @@ -1254,7 +1254,7 @@ func TestArmGantryCheckPlan(t *testing.T) { f.Name(): frame.NewPoseInFrame(frame.World, startPose), }, } - err = CheckPlan(f, executionState, nil, fs, math.Inf(1), logger) + err = CheckPlan(f, nil, executionState, nil, fs, math.Inf(1), logger) test.That(t, err, test.ShouldBeNil) }) t.Run("check plan with obstacle", func(t *testing.T) { @@ -1275,7 +1275,7 @@ func TestArmGantryCheckPlan(t *testing.T) { f.Name(): frame.NewPoseInFrame(frame.World, startPose), }, } - err = CheckPlan(f, executionState, worldState, fs, math.Inf(1), logger) + err = CheckPlan(f, nil, executionState, worldState, fs, math.Inf(1), logger) test.That(t, err, test.ShouldNotBeNil) }) } diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index 0da7fa66df5..d343ac16602 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -319,7 +319,7 @@ func TestPtgCheckPlan(t *testing.T) { localizationFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), }, } - err = CheckPlan(ackermanFrame, executionState, nil, tfFrameSystem, math.Inf(1), logger) + err = CheckPlan(ackermanFrame, localizationFrame, executionState, nil, tfFrameSystem, math.Inf(1), logger) test.That(t, err, test.ShouldBeNil) }) @@ -341,7 +341,7 @@ func TestPtgCheckPlan(t *testing.T) { localizationFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), }, } - err = CheckPlan(ackermanFrame, executionState, worldState, tfFrameSystem, math.Inf(1), logger) + err = CheckPlan(ackermanFrame, localizationFrame, executionState, worldState, tfFrameSystem, 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) }) @@ -387,7 +387,7 @@ func TestPtgCheckPlan(t *testing.T) { localizationFrame.Name(): plan.Path()[1][ackermanFrame.Name()], }, } - err = CheckPlan(ackermanFrame, executionState, worldState, tfFrameSystem, math.Inf(1), logger) + err = CheckPlan(ackermanFrame, localizationFrame, executionState, worldState, tfFrameSystem, math.Inf(1), logger) test.That(t, err, test.ShouldBeNil) }) @@ -411,7 +411,7 @@ func TestPtgCheckPlan(t *testing.T) { localizationFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), }, } - err = CheckPlan(ackermanFrame, executionState, worldState, tfFrameSystem, math.Inf(1), logger) + err = CheckPlan(ackermanFrame, localizationFrame, executionState, worldState, tfFrameSystem, 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) }) @@ -444,7 +444,7 @@ func TestPtgCheckPlan(t *testing.T) { localizationFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), }, } - err = CheckPlan(ackermanFrame, executionState, worldState, tfFrameSystem, math.Inf(1), logger) + err = CheckPlan(ackermanFrame, localizationFrame, executionState, worldState, tfFrameSystem, math.Inf(1), logger) test.That(t, err, test.ShouldBeNil) }) @@ -471,7 +471,7 @@ func TestPtgCheckPlan(t *testing.T) { localizationFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), }, } - err = CheckPlan(ackermanFrame, executionState, worldState, tfFrameSystem, math.Inf(1), logger) + err = CheckPlan(ackermanFrame, localizationFrame, executionState, worldState, tfFrameSystem, math.Inf(1), logger) test.That(t, err, test.ShouldBeNil) }) } From 34f52693f6c3f2ab36b3f719996a0fcfc88f0643 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 5 Jun 2024 23:17:09 -0400 Subject: [PATCH 064/126] add comment --- components/base/kinematicbase/ptgKinematics.go | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index b695668acc5..c590dafa46e 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -140,7 +140,10 @@ func wrapWithPTGKinematics( origin = originPIF.Pose() } startingState := baseState{currentInputs: zeroInput} - // construct localization frame + + // we intentionally set our OX, OY, OZ values to be greater than 1 since + // a possible orientation which a planner produces may have the following + // values: OZ: 1.0000000002, Theta: t. localizationFrame, err := referenceframe.NewPoseFrame( b.Name().ShortName()+"LocalizationFrame", []referenceframe.Limit{ From 9d6dbe71e10f0df60b31aa510fabf7b69d84d6bc Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 6 Jun 2024 10:59:34 -0400 Subject: [PATCH 065/126] add back state.Position case in switch stmt for NewCollisionConstraint --- motionplan/constraint.go | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/motionplan/constraint.go b/motionplan/constraint.go index 14d527c99cb..786afdd0f6b 100644 --- a/motionplan/constraint.go +++ b/motionplan/constraint.go @@ -335,6 +335,17 @@ func NewCollisionConstraint( return false } internalGeoms = internal.Geometries() + case state.Position != nil: + // If we didn't pass a Configuration, but we do have a Position, then get the geometries at the zero state and + // transform them to the Position + internal, err := state.Frame.Geometries(make([]referenceframe.Input, len(state.Frame.DoF()))) + if err != nil { + return false + } + movedGeoms := internal.Geometries() + for _, geom := range movedGeoms { + internalGeoms = append(internalGeoms, geom.Transform(state.Position)) + } default: return false } From 24c73498e255cd4b042975c63bb4e4fa0d1aae74 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 6 Jun 2024 12:02:16 -0400 Subject: [PATCH 066/126] add comment to fake_kinematics about why we have chosen the given limits for OX, OY, OZ, Theta --- components/base/kinematicbase/fake_kinematics.go | 3 +++ 1 file changed, 3 insertions(+) diff --git a/components/base/kinematicbase/fake_kinematics.go b/components/base/kinematicbase/fake_kinematics.go index 1a95196cf14..61b8f3d5c03 100644 --- a/components/base/kinematicbase/fake_kinematics.go +++ b/components/base/kinematicbase/fake_kinematics.go @@ -206,6 +206,9 @@ func WrapWithFakePTGKinematics( {Min: math.Inf(-1), Max: math.Inf(1)}, {Min: math.Inf(-1), Max: math.Inf(1)}, {Min: math.Inf(-1), Max: math.Inf(1)}, + // we intentionally set our OX, OY, OZ values to be greater than 1 since + // a possible orientation which a planner produces may have the following + // values: OZ: 1.0000000002, Theta: t. {Min: -1.5, Max: 1.5}, {Min: -1.5, Max: 1.5}, {Min: -1.5, Max: 1.5}, From 03ed80a60d8b9d63fb9408835027c40a188564e1 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 6 Jun 2024 12:16:38 -0400 Subject: [PATCH 067/126] remove comment --- motionplan/solverFrame.go | 1 - 1 file changed, 1 deletion(-) diff --git a/motionplan/solverFrame.go b/motionplan/solverFrame.go index 0821b2ff338..e6c438d5b06 100644 --- a/motionplan/solverFrame.go +++ b/motionplan/solverFrame.go @@ -51,7 +51,6 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string, if err != nil { return nil, err } - // this should always include world if len(solveFrameList) == 0 { return nil, errors.New("solveFrameList was empty") } From 3978b66c7750fa36f8ce4144c7437d6eab016afd Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 6 Jun 2024 12:17:03 -0400 Subject: [PATCH 068/126] update poseFrame interpolate method to use spatial.Interpolate --- referenceframe/frame.go | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 37bd9be0c2e..bdd7b822c4e 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -489,7 +489,24 @@ func (pf *poseFrame) Interpolate(from, to []Input, by float64) ([]Input, error) if err := pf.baseFrame.validInputs(to); err != nil { return nil, NewIncorrectInputLengthError(len(to), 7) } - return from, nil + 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 []Input{ + {interpolatedPose.Point().X}, + {interpolatedPose.Point().Y}, + {interpolatedPose.Point().Z}, + {interpolatedPose.Orientation().OrientationVectorRadians().OX}, + {interpolatedPose.Orientation().OrientationVectorRadians().OY}, + {interpolatedPose.Orientation().OrientationVectorRadians().OZ}, + {interpolatedPose.Orientation().OrientationVectorRadians().Theta}, + }, nil } // Geometries returns an object representing the 3D space associeted with the staticFrame. From 56636f43f6ab974ecf89a76f670a095400f4ae43 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 6 Jun 2024 13:48:31 -0400 Subject: [PATCH 069/126] update testptgcheckplan to have correct starting inputs --- motionplan/planManager.go | 2 -- motionplan/tpSpaceRRT_test.go | 14 +++++++++++--- services/motion/builtin/builtin_test.go | 2 +- services/motion/builtin/move_request.go | 5 ----- 4 files changed, 12 insertions(+), 11 deletions(-) diff --git a/motionplan/planManager.go b/motionplan/planManager.go index 58d9626e1cf..4c0858436fe 100644 --- a/motionplan/planManager.go +++ b/motionplan/planManager.go @@ -799,7 +799,6 @@ func (pm *planManager) planRelativeWaypoint(ctx context.Context, request *PlanRe return nil, err } goalPos := tf.(*referenceframe.PoseInFrame).Pose() - opt, err := pm.plannerSetupFromMoveRequest( startPose, goalPos, request.StartConfiguration, request.WorldState, request.ConstraintSpecs, request.Options, ) @@ -815,7 +814,6 @@ func (pm *planManager) planRelativeWaypoint(ctx context.Context, request *PlanRe pm.frame.fss = relativeOnlyFS pm.planOpts = opt - // set the goal pose opt.SetGoal(goalPos) // Build planner diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index d343ac16602..966184fb319 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -382,7 +382,7 @@ func TestPtgCheckPlan(t *testing.T) { executionState := ExecutionState{ plan: plan, index: 1, - currentInputs: inputs, // zero'd inputs are incorrect here + currentInputs: inputs, currentPose: map[string]*referenceframe.PoseInFrame{ localizationFrame.Name(): plan.Path()[1][ackermanFrame.Name()], }, @@ -461,14 +461,22 @@ func TestPtgCheckPlan(t *testing.T) { test.That(t, err, test.ShouldBeNil) pathPose := remainingPlan.Path()[0][ackermanFrame.Name()].Pose() - startPose := spatialmath.NewPose(r3.Vector{1000, 0, 0}, pathPose.Orientation()) + inputs[localizationFrame.Name()] = referenceframe.FloatsToInputs( + []float64{ + pathPose.Point().X, pathPose.Point().Y, pathPose.Point().Z, + pathPose.Orientation().OrientationVectorRadians().OX, + pathPose.Orientation().OrientationVectorRadians().OY, + pathPose.Orientation().OrientationVectorRadians().OZ, + pathPose.Orientation().OrientationVectorRadians().Theta, + }, + ) executionState := ExecutionState{ plan: plan, index: 2, currentInputs: inputs, currentPose: map[string]*referenceframe.PoseInFrame{ - localizationFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), + localizationFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, pathPose), }, } err = CheckPlan(ackermanFrame, localizationFrame, executionState, worldState, tfFrameSystem, math.Inf(1), logger) diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 39f6f4a8715..6da61c3d274 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -377,7 +377,7 @@ 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 500mm in front of it, triggering several replans. + // This base will always see an obstacle 800mm in front of it, triggering several replans. // However, enough replans should eventually get it to its goal. obstaclePosition := spatialmath.NewPoseFromPoint(r3.Vector{X: 800, Y: 0, Z: 0}) box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 20, Y: 20, Z: 10}, caseName) diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index f24344b7725..48d2af7daad 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -596,11 +596,6 @@ func (ms *builtIn) newMoveOnMapRequest( if err != nil { return nil, err } - limits = append(limits, referenceframe.Limit{Min: -1, Max: 1}) // Z - limits = append(limits, referenceframe.Limit{Min: -1, Max: 1}) // OX - limits = append(limits, referenceframe.Limit{Min: -1, Max: 1}) // OY - limits = append(limits, referenceframe.Limit{Min: -1, Max: 1}) // OZ - limits = append(limits, referenceframe.Limit{Min: -2 * math.Pi, Max: 2 * math.Pi}) // Theta // create a KinematicBase from the componentName component, ok := ms.components[req.ComponentName] From ba682e703da9245fba7c779efb414824e629d5f7 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 7 Jun 2024 13:54:12 -0400 Subject: [PATCH 070/126] debugging TestObstacleReplanningGlobe --- services/motion/builtin/builtin_test.go | 73 +++++++++++++------------ 1 file changed, 37 insertions(+), 36 deletions(-) diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 6da61c3d274..cf22142a131 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -315,6 +315,7 @@ func TestObstacleReplanningGlobe(t *testing.T) { gpsOrigin := geo.NewPoint(0, 0) dst := geo.NewPoint(gpsOrigin.Lat(), gpsOrigin.Lng()+1e-5) + // dst as r3.Vector with respect to gpsOrigin is: {111.92, 0, 0} epsilonMM := 15. type testCase struct { @@ -334,45 +335,45 @@ func TestObstacleReplanningGlobe(t *testing.T) { } extra := map[string]interface{}{"max_replans": 10, "max_ik_solutions": 1, "smooth_iter": 1} - extraNoReplan := map[string]interface{}{"max_replans": 0, "max_ik_solutions": 1, "smooth_iter": 1} + // extraNoReplan := map[string]interface{}{"max_replans": 0, "max_ik_solutions": 1, "smooth_iter": 1} // We set a flag here per test case so that detections are not returned the first time each vision service is called testCases := []testCase{ - { - name: "ensure no replan from discovered obstacles", - getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { - caseName := "test-case-1" - obstaclePosition := spatialmath.NewPoseFromPoint(r3.Vector{X: -1000, Y: -1000, Z: 0}) - box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 10, Y: 10, Z: 10}, caseName) - test.That(t, err, test.ShouldBeNil) - - detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) - test.That(t, err, test.ShouldBeNil) - - return []*viz.Object{detection}, nil - }, - expectedSuccess: true, - extra: extraNoReplan, - }, - { - name: "ensure replan due to obstacle collision", - getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { - caseName := "test-case-2" - // The camera is parented to the base. Thus, this will always see an obstacle 300mm in front of where the base is. - // Note: for createMoveOnGlobeEnvironment, 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) - test.That(t, err, test.ShouldBeNil) - - detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) - test.That(t, err, test.ShouldBeNil) - return []*viz.Object{detection}, nil - }, - expectedSuccess: false, - expectedErr: fmt.Sprintf("exceeded maximum number of replans: %d: plan failed", 0), - extra: extraNoReplan, - }, + // { + // name: "ensure no replan from discovered obstacles", + // getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { + // caseName := "test-case-1" + // obstaclePosition := spatialmath.NewPoseFromPoint(r3."Vec: ", Vector{X: -1000, Y: -1000, Z: 0}) + // box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 10, Y: 10, Z: 10}, caseName) + // test.That(t, err, test.ShouldBeNil) + + // detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) + // test.That(t, err, test.ShouldBeNil) + + // return []*viz.Object{detection}, nil + // }, + // expectedSuccess: true, + // extra: extraNoReplan, + // }, + // { + // name: "ensure replan due to obstacle collision", + // getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { + // caseName := "test-case-2" + // // The camera is parented to the base. Thus, this will always see an obstacle 300mm in front of where the base is. + // // Note: for createMoveOnGlobeEnvironment, 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) + // test.That(t, err, test.ShouldBeNil) + + // detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) + // test.That(t, err, test.ShouldBeNil) + // return []*viz.Object{detection}, nil + // }, + // expectedSuccess: false, + // expectedErr: fmt.Sprintf("exceeded maximum number of replans: %d: plan failed", 0), + // extra: extraNoReplan, + // }, { name: "ensure replan reaching goal", getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { From ee14bf1bfcd176d6ef532fa5925fcf203d335b3d Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 7 Jun 2024 14:51:13 -0400 Subject: [PATCH 071/126] fixed TestObstacleReplanningGlobe --- services/motion/builtin/builtin_test.go | 74 ++++++++++++------------- 1 file changed, 37 insertions(+), 37 deletions(-) diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 840b5776256..3491dacd855 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -336,52 +336,52 @@ func TestObstacleReplanningGlobe(t *testing.T) { } extra := map[string]interface{}{"max_replans": 10, "max_ik_solutions": 1, "smooth_iter": 1} - // extraNoReplan := map[string]interface{}{"max_replans": 0, "max_ik_solutions": 1, "smooth_iter": 1} + extraNoReplan := map[string]interface{}{"max_replans": 0, "max_ik_solutions": 1, "smooth_iter": 1} // We set a flag here per test case so that detections are not returned the first time each vision service is called testCases := []testCase{ - // { - // name: "ensure no replan from discovered obstacles", - // getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { - // caseName := "test-case-1" - // obstaclePosition := spatialmath.NewPoseFromPoint(r3."Vec: ", Vector{X: -1000, Y: -1000, Z: 0}) - // box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 10, Y: 10, Z: 10}, caseName) - // test.That(t, err, test.ShouldBeNil) - - // detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) - // test.That(t, err, test.ShouldBeNil) - - // return []*viz.Object{detection}, nil - // }, - // expectedSuccess: true, - // extra: extraNoReplan, - // }, - // { - // name: "ensure replan due to obstacle collision", - // getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { - // caseName := "test-case-2" - // // The camera is parented to the base. Thus, this will always see an obstacle 300mm in front of where the base is. - // // Note: for createMoveOnGlobeEnvironment, 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) - // test.That(t, err, test.ShouldBeNil) - - // detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) - // test.That(t, err, test.ShouldBeNil) - // return []*viz.Object{detection}, nil - // }, - // expectedSuccess: false, - // expectedErr: fmt.Sprintf("exceeded maximum number of replans: %d: plan failed", 0), - // extra: extraNoReplan, - // }, + { + name: "ensure no replan from discovered obstacles", + getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { + caseName := "test-case-1" + obstaclePosition := spatialmath.NewPoseFromPoint(r3.Vector{X: -1000, Y: -1000, Z: 0}) + box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 10, Y: 10, Z: 10}, caseName) + test.That(t, err, test.ShouldBeNil) + + detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) + test.That(t, err, test.ShouldBeNil) + + return []*viz.Object{detection}, nil + }, + expectedSuccess: true, + extra: extraNoReplan, + }, + { + name: "ensure replan due to obstacle collision", + getPCfunc: func(ctx context.Context, cameraName string, extra map[string]interface{}) ([]*viz.Object, error) { + caseName := "test-case-2" + // The camera is parented to the base. Thus, this will always see an obstacle 300mm in front of where the base is. + // Note: for createMoveOnGlobeEnvironment, 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) + test.That(t, err, test.ShouldBeNil) + + detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) + test.That(t, err, test.ShouldBeNil) + return []*viz.Object{detection}, nil + }, + expectedSuccess: false, + expectedErr: fmt.Sprintf("exceeded maximum number of replans: %d: plan failed", 0), + extra: extraNoReplan, + }, { 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. // However, enough replans should eventually get it to its goal. - obstaclePosition := spatialmath.NewPoseFromPoint(r3.Vector{X: 800, Y: 0, Z: 0}) + obstaclePosition := spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: -800, Z: 0}) box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 20, Y: 20, Z: 10}, caseName) test.That(t, err, test.ShouldBeNil) From f43114d57f540da2afc4e45a1e2ac4f4ed86625c Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 12 Jun 2024 10:07:18 -0400 Subject: [PATCH 072/126] rename absoluteFS to localizingFS --- services/motion/builtin/move_request.go | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 3f8f7878f84..cdb00ea7ddd 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -66,7 +66,7 @@ type moveRequest struct { obstacleDetectors map[vision.Service][]resource.Name replanCostFactor float64 fsService framesystem.Service - absoluteFS referenceframe.FrameSystem + localizaingFS referenceframe.FrameSystem executeBackgroundWorkers *sync.WaitGroup responseChan chan moveResponse @@ -202,7 +202,7 @@ func (mr *moveRequest) getTransientDetections( label += "_" + geometry.Label() } geometry.SetLabel(label) - tf, err := mr.absoluteFS.Transform( + tf, err := mr.localizaingFS.Transform( inputMap, referenceframe.NewGeometriesInFrame(camName.ShortName(), []spatialmath.Geometry{geometry}), referenceframe.World, @@ -289,7 +289,7 @@ func (mr *moveRequest) obstaclesIntersectPlan( mr.kinematicBase.LocalizationFrame(), executionState, worldState, // detected obstacles by this instance of camera + service - mr.absoluteFS, + mr.localizaingFS, lookAheadDistanceMM, mr.planRequest.Logger, ); err != nil { @@ -302,7 +302,7 @@ func (mr *moveRequest) obstaclesIntersectPlan( } func (mr *moveRequest) createInputMap(baseExecutionState motionplan.ExecutionState) map[string][]referenceframe.Input { - inputMap := referenceframe.StartPositions(mr.absoluteFS) + inputMap := referenceframe.StartPositions(mr.localizaingFS) inputMap[mr.kinematicBase.Name().ShortName()] = baseExecutionState.CurrentInputs()[mr.kinematicBase.Name().Name] inputMap[mr.kinematicBase.LocalizationFrame().Name()] = referenceframe.FloatsToInputs([]float64{ baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().X, @@ -707,12 +707,12 @@ func (ms *builtIn) createBaseMoveRequest( } // create collision checking fs - collisionFS := referenceframe.NewEmptyFrameSystem("collisionFS") - err = collisionFS.AddFrame(kb.LocalizationFrame(), collisionFS.World()) + localizingFS := referenceframe.NewEmptyFrameSystem("localizingFS") + err = localizingFS.AddFrame(kb.LocalizationFrame(), localizingFS.World()) if err != nil { return nil, err } - err = collisionFS.MergeFrameSystem(baseOnlyFS, kb.LocalizationFrame()) + err = localizingFS.MergeFrameSystem(baseOnlyFS, kb.LocalizationFrame()) if err != nil { return nil, err } @@ -793,7 +793,7 @@ func (ms *builtIn) createBaseMoveRequest( replanCostFactor: valExtra.replanCostFactor, obstacleDetectors: obstacleDetectors, fsService: ms.fsService, - absoluteFS: collisionFS, + localizaingFS: localizingFS, executeBackgroundWorkers: &backgroundWorkers, From 34352cdf3879dc9fffd7f2b0210a5c047a9389b9 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 12 Jun 2024 10:28:39 -0400 Subject: [PATCH 073/126] update naming --- services/motion/builtin/move_on_map_test.go | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/services/motion/builtin/move_on_map_test.go b/services/motion/builtin/move_on_map_test.go index d7032e8293c..b10a5c34fa8 100644 --- a/services/motion/builtin/move_on_map_test.go +++ b/services/motion/builtin/move_on_map_test.go @@ -589,7 +589,7 @@ func TestMoveOnMapStaticObs(t *testing.T) { }, nil, ) test.That(t, err, test.ShouldBeNil) - currentInputs := referenceframe.StartPositions(mr.absoluteFS) + currentInputs := referenceframe.StartPositions(mr.localizaingFS) currentInputs[mr.kinematicBase.LocalizationFrame().Name()] = referenceframe.FloatsToInputs( []float64{0.58772e3, -0.80826e3, 0, 0, 0, 1, 0}, ) @@ -610,7 +610,7 @@ func TestMoveOnMapStaticObs(t *testing.T) { mr.kinematicBase.LocalizationFrame(), executionState, wrldSt, - mr.absoluteFS, + mr.localizaingFS, lookAheadDistanceMM, logger, ) From d4e1c11c68a7e7b53bf319de992cb7646f519978 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 17 Jun 2024 15:46:30 -0400 Subject: [PATCH 074/126] generate seed map with all input-enabled components --- services/motion/builtin/move_request.go | 21 ++++++++++++++++----- 1 file changed, 16 insertions(+), 5 deletions(-) diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 18cf7d1cf6c..6990fb19ed4 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -184,7 +184,10 @@ func (mr *moveRequest) getTransientDetections( if err != nil { return nil, err } - inputMap := mr.createInputMap(baseExecutionState) + inputMap, err := mr.createInputMap(ctx, baseExecutionState) + if err != nil { + return nil, err + } inputMap[mr.kinematicBase.Name().ShortName()] = make([]referenceframe.Input, len(mr.kinematicBase.Kinematics().DoF())) detections, err := visSrvc.GetObjectPointClouds(ctx, camName.Name, nil) @@ -267,7 +270,10 @@ 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 := mr.createInputMap(baseExecutionState) + inputMap, err := mr.createInputMap(ctx, baseExecutionState) + if err != nil { + return state.ExecuteResponse{}, err + } executionState, err := motionplan.NewExecutionState( baseExecutionState.Plan(), baseExecutionState.Index(), @@ -301,8 +307,13 @@ func (mr *moveRequest) obstaclesIntersectPlan( return state.ExecuteResponse{}, nil } -func (mr *moveRequest) createInputMap(baseExecutionState motionplan.ExecutionState) map[string][]referenceframe.Input { - inputMap := referenceframe.StartPositions(mr.localizaingFS) +func (mr *moveRequest) createInputMap( + ctx context.Context, baseExecutionState motionplan.ExecutionState, +) (map[string][]referenceframe.Input, error) { + inputMap, _, err := mr.fsService.CurrentInputs(ctx) + if err != nil { + return nil, err + } inputMap[mr.kinematicBase.Name().ShortName()] = baseExecutionState.CurrentInputs()[mr.kinematicBase.Name().Name] inputMap[mr.kinematicBase.LocalizationFrame().Name()] = referenceframe.FloatsToInputs([]float64{ baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().X, @@ -313,7 +324,7 @@ func (mr *moveRequest) createInputMap(baseExecutionState motionplan.ExecutionSta baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().OZ, baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().Theta, }) - return inputMap + return inputMap, nil } func kbOptionsFromCfg(motionCfg *validatedMotionConfiguration, validatedExtra validatedExtra) kinematicbase.Options { From 3fc360b6b7a97c05784c1877225dee41bf57e3da Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 17 Jun 2024 15:47:14 -0400 Subject: [PATCH 075/126] use zeroth index of plan to construct our zeroCG --- motionplan/check.go | 83 +++++++++++++++++++++------------------------ 1 file changed, 38 insertions(+), 45 deletions(-) diff --git a/motionplan/check.go b/motionplan/check.go index 1a8cbe1e88d..7fbccb36c72 100644 --- a/motionplan/check.go +++ b/motionplan/check.go @@ -119,10 +119,6 @@ func checkPlanRelative( return poseInWorld, nil } - plan := executionState.Plan() - wayPointIdx := executionState.Index() - startingInputs := executionState.currentInputs - currentInputs := executionState.CurrentInputs() currentPoses := executionState.CurrentPoses() if currentPoses == nil { @@ -133,7 +129,8 @@ func checkPlanRelative( return errors.New("LocalizationFrame not found in current pose map") } - sf := sfPlanner.frame + plan := executionState.Plan() + wayPointIdx := executionState.Index() // Check that path pose is valid stepEndPiF, ok := plan.Path()[wayPointIdx][checkFrame.Name()] @@ -150,34 +147,11 @@ func checkPlanRelative( 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 { - return err - } - - // 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()) - - planStartPiF, ok := plan.Path()[wayPointIdx][checkFrame.Name()] + 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) + planStartPoseWorld, err := toWorld(planStartPiF, plan.Trajectory()[0]) if err != nil { return err } @@ -190,14 +164,22 @@ func checkPlanRelative( return err } - overWrittenInputs := startingInputs - overWrittenInputs[checkFrame.Name()] = make([]referenceframe.Input, len(checkFrame.DoF())) + startingInputs := plan.Trajectory()[0] + startingInputs[localizationFrame.Name()] = referenceframe.FloatsToInputs([]float64{ + planStartPoseWorld.Pose().Point().X, + planStartPoseWorld.Pose().Point().Y, + planStartPoseWorld.Pose().Point().Z, + planStartPoseWorld.Pose().Orientation().OrientationVectorRadians().OX, + planStartPoseWorld.Pose().Orientation().OrientationVectorRadians().OY, + planStartPoseWorld.Pose().Orientation().OrientationVectorRadians().OZ, + planStartPoseWorld.Pose().Orientation().OrientationVectorRadians().Theta, + }) // 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(), - overWrittenInputs, + startingInputs, worldState, nil, nil, // no pb.Constraints @@ -206,6 +188,7 @@ func checkPlanRelative( return err } + sf := sfPlanner.frame 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 @@ -214,26 +197,36 @@ func checkPlanRelative( return err } - poses, err := plan.Path().GetFramePoses(checkFrame.Name()) + currentWayPointTraj := plan.Trajectory()[wayPointIdx] + currentWayPointTraj[localizationFrame.Name()] = currentInputs[localizationFrame.Name()] + arcEndInputs, err := sf.mapToSlice(currentWayPointTraj) if err != nil { return err } - currentWayPointTraj := plan.Trajectory()[wayPointIdx] - currentWayPointTraj[localizationFrame.Name()] = referenceframe.FloatsToInputs([]float64{ - poses[wayPointIdx].Point().X, - poses[wayPointIdx].Point().Y, - poses[wayPointIdx].Point().Z, - poses[wayPointIdx].Orientation().OrientationVectorRadians().OX, - poses[wayPointIdx].Orientation().OrientationVectorRadians().OY, - poses[wayPointIdx].Orientation().OrientationVectorRadians().OZ, - poses[wayPointIdx].Orientation().OrientationVectorRadians().Theta, - }) - arcEndInputs, err := sf.mapToSlice(currentWayPointTraj) + 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 { return err } + // 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) // pre-pend to segments so we can connect to the input we have not finished actuating yet From 3409b4057eb86c29732c5698ec2bcd6f28256a07 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 17 Jun 2024 15:47:43 -0400 Subject: [PATCH 076/126] noramlize the orientation vector limits when set to infinity --- .../base/kinematicbase/fake_kinematics.go | 18 +-------- .../base/kinematicbase/ptgKinematics.go | 15 +------- motionplan/tpSpaceRRT_test.go | 14 +------ referenceframe/frame.go | 37 ++++++++++--------- 4 files changed, 22 insertions(+), 62 deletions(-) diff --git a/components/base/kinematicbase/fake_kinematics.go b/components/base/kinematicbase/fake_kinematics.go index 61b8f3d5c03..61e0d79eb21 100644 --- a/components/base/kinematicbase/fake_kinematics.go +++ b/components/base/kinematicbase/fake_kinematics.go @@ -5,7 +5,6 @@ package kinematicbase import ( "context" "errors" - "math" "sync" "time" @@ -200,22 +199,7 @@ func WrapWithFakePTGKinematics( } // construct localization frame - localizationFrame, err := referenceframe.NewPoseFrame( - b.Name().ShortName()+"LocalizationFrame", - []referenceframe.Limit{ - {Min: math.Inf(-1), Max: math.Inf(1)}, - {Min: math.Inf(-1), Max: math.Inf(1)}, - {Min: math.Inf(-1), Max: math.Inf(1)}, - // we intentionally set our OX, OY, OZ values to be greater than 1 since - // a possible orientation which a planner produces may have the following - // values: OZ: 1.0000000002, Theta: t. - {Min: -1.5, Max: 1.5}, - {Min: -1.5, Max: 1.5}, - {Min: -1.5, Max: 1.5}, - {Min: -2 * math.Pi, Max: 2 * math.Pi}, - }, - nil, - ) + localizationFrame, err := referenceframe.NewPoseFrame(b.Name().ShortName()+"LocalizationFrame", nil) if err != nil { return nil, err } diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index c590dafa46e..eb8cc0ade06 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -7,7 +7,6 @@ package kinematicbase import ( "context" "errors" - "math" "sync" "go.viam.com/rdk/components/base" @@ -144,19 +143,7 @@ func wrapWithPTGKinematics( // we intentionally set our OX, OY, OZ values to be greater than 1 since // a possible orientation which a planner produces may have the following // values: OZ: 1.0000000002, Theta: t. - localizationFrame, err := referenceframe.NewPoseFrame( - b.Name().ShortName()+"LocalizationFrame", - []referenceframe.Limit{ - {Min: math.Inf(-1), Max: math.Inf(1)}, - {Min: math.Inf(-1), Max: math.Inf(1)}, - {Min: math.Inf(-1), Max: math.Inf(1)}, - {Min: -1.5, Max: 1.5}, - {Min: -1.5, Max: 1.5}, - {Min: -1.5, Max: 1.5}, - {Min: -2 * math.Pi, Max: 2 * math.Pi}, - }, - nil, - ) + localizationFrame, err := referenceframe.NewPoseFrame(b.Name().ShortName()+"LocalizationFrame", nil) if err != nil { return nil, err } diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index 6bd36cbc52e..259e100064b 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -288,19 +288,7 @@ func TestPtgCheckPlan(t *testing.T) { // NOTE: WE NEED TO ADD AN EXECUTION FRAME TO THE CHECKING FRAMESYSTEM SINCE WE ONLY WANT TO RELY ON USING INPUTS tfFrameSystem := referenceframe.NewEmptyFrameSystem("transformFS") - localizationFrame, err := referenceframe.NewPoseFrame( - "ackframeLocalizationFrame", - []referenceframe.Limit{ - {Min: math.Inf(-1), Max: math.Inf(1)}, - {Min: math.Inf(-1), Max: math.Inf(1)}, - {Min: math.Inf(-1), Max: math.Inf(1)}, - {Min: -1.5, Max: 1.5}, - {Min: -1.5, Max: 1.5}, - {Min: -1.5, Max: 1.5}, - {Min: -2 * math.Pi, Max: 2 * math.Pi}, - }, - nil, - ) + localizationFrame, err := referenceframe.NewPoseFrame("ackframeLocalizationFrame", nil) test.That(t, err, test.ShouldBeNil) err = tfFrameSystem.AddFrame(localizationFrame, tfFrameSystem.World()) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index bdd7b822c4e..0894ab303c3 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -451,13 +451,25 @@ type poseFrame struct { geometry spatial.Geometry } -// NewPoseFrame created a frame with 7 degrees of freedom, x, y, z, OX, OY, OZ, and theta in radians. -func NewPoseFrame(name string, limits []Limit, geometry spatial.Geometry) (Frame, error) { - if len(limits) != 7 { - return nil, - errors.Errorf("Must have 7DOF state (x, y, z, OX, OY, OZ, theta) to create 6DFrame, have %d dof", len(limits)) +// 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) { + orientationVector := spatial.OrientationVector{ + OX: math.Inf(1), + OY: math.Inf(1), + OZ: math.Inf(1), + Theta: math.Pi, + } + orientationVector.Normalize() + 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: -orientationVector.OX, Max: orientationVector.OX}, // OX + {Min: -orientationVector.OY, Max: orientationVector.OY}, // OY + {Min: -orientationVector.OZ, Max: orientationVector.OZ}, // OZ + {Min: -orientationVector.Theta, Max: orientationVector.Theta}, // Theta } - return &poseFrame{ &baseFrame{name, limits}, geometry, @@ -528,18 +540,7 @@ func (pf *poseFrame) DoF() []Limit { // MarshalJSON serializes a Model. func (pf *poseFrame) MarshalJSON() ([]byte, error) { - temp := LinkConfig{ - ID: pf.name, - } - - if pf.geometry != nil { - var err error - temp.Geometry, err = spatial.NewGeometryConfig(pf.geometry) - if err != nil { - return nil, err - } - } - return json.Marshal(temp) + return nil, errors.New("serializing a poseFrame is currently not supported") } // InputFromProtobuf converts pb.JointPosition to inputs. From 3973ef71a99425a78721651c097e798ad7c55cb5 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Tue, 18 Jun 2024 14:18:48 -0400 Subject: [PATCH 077/126] introduce helper methods PoseToInputs and InputsToPose --- motionplan/check.go | 20 ++--------- referenceframe/frame.go | 46 +++++++++++++++---------- services/motion/builtin/move_request.go | 12 ++----- 3 files changed, 33 insertions(+), 45 deletions(-) diff --git a/motionplan/check.go b/motionplan/check.go index 7fbccb36c72..c95f3db0b72 100644 --- a/motionplan/check.go +++ b/motionplan/check.go @@ -165,15 +165,7 @@ func checkPlanRelative( } startingInputs := plan.Trajectory()[0] - startingInputs[localizationFrame.Name()] = referenceframe.FloatsToInputs([]float64{ - planStartPoseWorld.Pose().Point().X, - planStartPoseWorld.Pose().Point().Y, - planStartPoseWorld.Pose().Point().Z, - planStartPoseWorld.Pose().Orientation().OrientationVectorRadians().OX, - planStartPoseWorld.Pose().Orientation().OrientationVectorRadians().OY, - planStartPoseWorld.Pose().Orientation().OrientationVectorRadians().OZ, - planStartPoseWorld.Pose().Orientation().OrientationVectorRadians().Theta, - }) + startingInputs[localizationFrame.Name()] = referenceframe.PoseToInputs(planStartPoseWorld.Pose()) // 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( @@ -261,15 +253,7 @@ func checkPlanRelative( } } - startInputs[localizationFrame.Name()] = referenceframe.FloatsToInputs( - []float64{ - lastArcEndPose.Point().X, lastArcEndPose.Point().Y, lastArcEndPose.Point().Z, - lastArcEndPose.Orientation().OrientationVectorRadians().OX, - lastArcEndPose.Orientation().OrientationVectorRadians().OY, - lastArcEndPose.Orientation().OrientationVectorRadians().OZ, - lastArcEndPose.Orientation().OrientationVectorRadians().Theta, - }, - ) + startInputs[localizationFrame.Name()] = referenceframe.PoseToInputs(lastArcEndPose) nextInputs := plan.Trajectory()[i] nextInputs[localizationFrame.Name()] = startInputs[localizationFrame.Name()] segment, err := createSegment(sf, lastArcEndPose, thisArcEndPose, startInputs, nextInputs) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 0894ab303c3..10ecd4296f5 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -482,15 +482,7 @@ func (pf *poseFrame) Transform(inputs []Input) (spatial.Pose, error) { if err := pf.baseFrame.validInputs(inputs); err != nil { return nil, NewIncorrectInputLengthError(len(inputs), 7) } - 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 + return InputsToPose(inputs), nil } // Interpolate interpolates the given amount between the two sets of inputs. @@ -510,15 +502,7 @@ func (pf *poseFrame) Interpolate(from, to []Input, by float64) ([]Input, error) return nil, err } interpolatedPose := spatial.Interpolate(fromPose, toPose, by) - return []Input{ - {interpolatedPose.Point().X}, - {interpolatedPose.Point().Y}, - {interpolatedPose.Point().Z}, - {interpolatedPose.Orientation().OrientationVectorRadians().OX}, - {interpolatedPose.Orientation().OrientationVectorRadians().OY}, - {interpolatedPose.Orientation().OrientationVectorRadians().OZ}, - {interpolatedPose.Orientation().OrientationVectorRadians().Theta}, - }, nil + return PoseToInputs(interpolatedPose), nil } // Geometries returns an object representing the 3D space associeted with the staticFrame. @@ -560,3 +544,29 @@ func (pf *poseFrame) ProtobufFromInput(input []Input) *pb.JointPositions { } return &pb.JointPositions{Values: n} } + +// PoseToInputs is a convience method for turning poses into inputs +// We note that the orientation of the pose will be understood +// as OrientationVectorRadians. +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, + }) +} + +// InputsToPose is a convience method for turning inputs into a spatial.Pose +func InputsToPose(inputs []Input) spatial.Pose { + 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, + }, + ) +} diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 6990fb19ed4..7d8924467a8 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -315,15 +315,9 @@ func (mr *moveRequest) createInputMap( return nil, err } inputMap[mr.kinematicBase.Name().ShortName()] = baseExecutionState.CurrentInputs()[mr.kinematicBase.Name().Name] - inputMap[mr.kinematicBase.LocalizationFrame().Name()] = referenceframe.FloatsToInputs([]float64{ - baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().X, - baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().Y, - baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Point().Z, - baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().OX, - baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().OY, - baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().OZ, - baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose().Orientation().OrientationVectorRadians().Theta, - }) + inputMap[mr.kinematicBase.LocalizationFrame().Name()] = referenceframe.PoseToInputs( + baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose(), + ) return inputMap, nil } From 0241c2272933b0044502e446219cd2b9dfe42da2 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Tue, 25 Jun 2024 14:17:17 -0400 Subject: [PATCH 078/126] finished wrapperFrame impl with passing motion service tests --- motionplan/check.go | 109 +++++----- motionplan/motionPlanner_test.go | 4 +- motionplan/tpSpaceRRT_test.go | 229 -------------------- referenceframe/frame.go | 6 +- services/motion/builtin/builtin_test.go | 204 +++++++++++++++++ services/motion/builtin/move_on_map_test.go | 41 +++- services/motion/builtin/move_request.go | 91 +++++--- services/motion/builtin/wrapperFrame.go | 158 ++++++++++++++ services/motion/explore/explore.go | 1 - 9 files changed, 517 insertions(+), 326 deletions(-) create mode 100644 services/motion/builtin/wrapperFrame.go diff --git a/motionplan/check.go b/motionplan/check.go index c95f3db0b72..61bbb5c3cdc 100644 --- a/motionplan/check.go +++ b/motionplan/check.go @@ -14,11 +14,16 @@ import ( "go.viam.com/rdk/spatialmath" ) +var ( + checkFrameNotInPathErr = errors.New("checkFrame given not in plan.Path() map") + ptgDoFLen = 4 +) + // 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. func CheckPlan( - checkFrame, localizationFrame referenceframe.Frame, // TODO(RSDK-7421): remove this + checkFrame referenceframe.Frame, // TODO(RSDK-7421): remove this executionState ExecutionState, worldState *referenceframe.WorldState, fs referenceframe.FrameSystem, @@ -50,19 +55,16 @@ func CheckPlan( 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. - // The solver frame will have had its PTGs filled in the newPlanManager() call, if applicable. - if sfPlanner.useTPspace { - if localizationFrame == nil { - return errors.New("cannot have nil localizationFrame when checking PTG plans") - } - return checkPlanRelative(checkFrame, localizationFrame, executionState, worldState, fs, lookAheadDistanceMM, sfPlanner) + // Currently this is only TP-space, so we check if the DoF of checkFrame is 11, indication that is a wrapper frame, + // housing the ptgFrame with 4 Dof and localization frame with 7 DoF. + if len(checkFrame.DoF()) == 11 { + return checkPlanRelative(checkFrame, executionState, worldState, fs, lookAheadDistanceMM, sfPlanner) } return checkPlanAbsolute(checkFrame, executionState, worldState, fs, lookAheadDistanceMM, sfPlanner) } func checkPlanRelative( - checkFrame, localizationFrame referenceframe.Frame, // TODO(RSDK-7421): remove this + checkFrame referenceframe.Frame, // TODO(RSDK-7421): remove this executionState ExecutionState, worldState *referenceframe.WorldState, fs referenceframe.FrameSystem, @@ -119,59 +121,33 @@ func checkPlanRelative( return poseInWorld, nil } - currentInputs := executionState.CurrentInputs() - currentPoses := executionState.CurrentPoses() - if currentPoses == nil { - return errors.New("executionState had nil return from CurrentPoses") - } - currentPoseIF, ok := currentPoses[localizationFrame.Name()] - if !ok { - return errors.New("LocalizationFrame not found in current pose map") - } - plan := executionState.Plan() - wayPointIdx := executionState.Index() - - // Check that 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]) - if err != nil { - return err - } - - currentPoseInWorld, err := toWorld(currentPoseIF, currentInputs) - if err != nil { - return err - } + // determine plan's starting pose planStartPiF, ok := plan.Path()[0][checkFrame.Name()] if !ok { - return errors.New("check frame given not in plan Path map") + return checkFrameNotInPathErr // THIS ERROR APPEARS TWICE, SO WE CAN MAKE IT INTO A VAR } planStartPoseWorld, err := toWorld(planStartPiF, plan.Trajectory()[0]) if err != nil { return err } + + // determine plan's ending pose planEndPiF, ok := plan.Path()[len(plan.Path())-1][checkFrame.Name()] if !ok { - return errors.New("check frame given not in plan Path map") + return checkFrameNotInPathErr } planEndPoseWorld, err := toWorld(planEndPiF, plan.Trajectory()[len(plan.Path())-1]) if err != nil { return err } - startingInputs := plan.Trajectory()[0] - startingInputs[localizationFrame.Name()] = referenceframe.PoseToInputs(planStartPoseWorld.Pose()) - // 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, + plan.Trajectory()[0], worldState, nil, nil, // no pb.Constraints @@ -180,8 +156,11 @@ func checkPlanRelative( return err } + currentInputs := executionState.CurrentInputs() + wayPointIdx := executionState.Index() sf := sfPlanner.frame - segments := make([]*ik.Segment, 0, len(plan.Path())-wayPointIdx) + + // construct first segmenet'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) @@ -189,16 +168,43 @@ func checkPlanRelative( return err } + // construct first segmenet's end configuration currentWayPointTraj := plan.Trajectory()[wayPointIdx] - currentWayPointTraj[localizationFrame.Name()] = currentInputs[localizationFrame.Name()] arcEndInputs, err := sf.mapToSlice(currentWayPointTraj) if err != nil { return err } + // construct first segmenet'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("checkFrame not found in current pose map") + } + currentPoseInWorld, err := toWorld(currentPoseIF, currentInputs) + if err != nil { + return err + } + + // construct first segmenet's endPosition + // Check that path pose is valid + stepEndPiF, ok := plan.Path()[wayPointIdx][checkFrame.Name()] + if !ok { + return checkFrameNotInPathErr + } + + expectedArcEndInWorld, err := toWorld(stepEndPiF, plan.Trajectory()[wayPointIdx]) + 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") + return checkFrameNotInPathErr } fullArcPose, err := checkFrame.Transform(arcInputs) if err != nil { @@ -211,6 +217,7 @@ func checkPlanRelative( if !ok { return errors.New("given checkFrame had no inputs in CurrentInputs map") } + poseThroughArc, err := checkFrame.Transform(frameCurrentInputs) if err != nil { return err @@ -218,10 +225,9 @@ func checkPlanRelative( remainingArcPose := spatialmath.PoseBetween(poseThroughArc, fullArcPose) expectedCurrentPose := spatialmath.PoseBetweenInverse(remainingArcPose, expectedArcEndInWorld.Pose()) errorState := spatialmath.PoseBetween(expectedCurrentPose, currentPoseInWorld.Pose()) - currentArcEndPose := spatialmath.Compose(expectedArcEndInWorld.Pose(), errorState) - // pre-pend to segments so we can connect to the input we have not finished actuating yet + segments := make([]*ik.Segment, 0, len(plan.Path())-wayPointIdx) segments = append(segments, &ik.Segment{ StartPosition: currentPoseInWorld.Pose(), EndPosition: currentArcEndPose, @@ -236,26 +242,25 @@ func checkPlanRelative( 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") + return checkFrameNotInPathErr } thisArcEndPoseInWorld, err := toWorld(thisArcEndPoseTf, 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())) + // Starting inputs for relative frames should be all-zero + correctedInputs := make([]referenceframe.Input, ptgDoFLen) + correctedInputs = append(correctedInputs, v[ptgDoFLen:]...) + startInputs[k] = correctedInputs } else { startInputs[k] = v } } - - startInputs[localizationFrame.Name()] = referenceframe.PoseToInputs(lastArcEndPose) nextInputs := plan.Trajectory()[i] - nextInputs[localizationFrame.Name()] = startInputs[localizationFrame.Name()] segment, err := createSegment(sf, lastArcEndPose, thisArcEndPose, startInputs, nextInputs) if err != nil { return err diff --git a/motionplan/motionPlanner_test.go b/motionplan/motionPlanner_test.go index 5c03c63cefd..0e29a7c5c01 100644 --- a/motionplan/motionPlanner_test.go +++ b/motionplan/motionPlanner_test.go @@ -1256,7 +1256,7 @@ func TestArmGantryCheckPlan(t *testing.T) { f.Name(): frame.NewPoseInFrame(frame.World, startPose), }, } - err = CheckPlan(f, nil, executionState, nil, fs, math.Inf(1), logger) + err = CheckPlan(f, executionState, nil, fs, math.Inf(1), logger) test.That(t, err, test.ShouldBeNil) }) t.Run("check plan with obstacle", func(t *testing.T) { @@ -1277,7 +1277,7 @@ func TestArmGantryCheckPlan(t *testing.T) { f.Name(): frame.NewPoseInFrame(frame.World, startPose), }, } - err = CheckPlan(f, nil, executionState, worldState, fs, math.Inf(1), logger) + err = CheckPlan(f, executionState, worldState, fs, math.Inf(1), logger) test.That(t, err, test.ShouldNotBeNil) }) } diff --git a/motionplan/tpSpaceRRT_test.go b/motionplan/tpSpaceRRT_test.go index 8ce232ee417..0218fcce0fc 100644 --- a/motionplan/tpSpaceRRT_test.go +++ b/motionplan/tpSpaceRRT_test.go @@ -6,7 +6,6 @@ import ( "context" "math" "math/rand" - "strings" "testing" "github.com/golang/geo/r3" @@ -258,234 +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}, "roverGeom") - 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.NewZeroPose() - inputs := plan.Trajectory()[0] - - // NOTE: WE NEED TO ADD AN EXECUTION FRAME TO THE CHECKING FRAMESYSTEM SINCE WE ONLY WANT TO RELY ON USING INPUTS - tfFrameSystem := referenceframe.NewEmptyFrameSystem("transformFS") - localizationFrame, err := referenceframe.NewPoseFrame("ackframeLocalizationFrame", nil) - test.That(t, err, test.ShouldBeNil) - - err = tfFrameSystem.AddFrame(localizationFrame, tfFrameSystem.World()) - test.That(t, err, test.ShouldBeNil) - - err = tfFrameSystem.MergeFrameSystem(fs, localizationFrame) - test.That(t, err, test.ShouldBeNil) - inputs[localizationFrame.Name()] = referenceframe.FloatsToInputs([]float64{0, 0, 0, 0, 0, 1, 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{ - localizationFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), - }, - } - err = CheckPlan(ackermanFrame, localizationFrame, executionState, nil, tfFrameSystem, 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{ - localizationFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), - }, - } - err = CheckPlan(ackermanFrame, localizationFrame, executionState, worldState, tfFrameSystem, 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 = tfFrameSystem.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 = tfFrameSystem.AddFrame(cameraFrame, cameraOriginFrame) - test.That(t, err, test.ShouldBeNil) - inputs[cameraFrame.Name()] = referenceframe.FloatsToInputs(make([]float64, len(cameraFrame.DoF()))) - - 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{ - localizationFrame.Name(): plan.Path()[1][ackermanFrame.Name()], - }, - } - err = CheckPlan(ackermanFrame, localizationFrame, executionState, worldState, tfFrameSystem, 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{ - localizationFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), - }, - } - err = CheckPlan(ackermanFrame, localizationFrame, executionState, worldState, tfFrameSystem, 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) - }) - - 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{ - localizationFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, startPose), - }, - } - err = CheckPlan(ackermanFrame, localizationFrame, executionState, worldState, tfFrameSystem, 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() - inputs[localizationFrame.Name()] = referenceframe.FloatsToInputs( - []float64{ - pathPose.Point().X, pathPose.Point().Y, pathPose.Point().Z, - pathPose.Orientation().OrientationVectorRadians().OX, - pathPose.Orientation().OrientationVectorRadians().OY, - pathPose.Orientation().OrientationVectorRadians().OZ, - pathPose.Orientation().OrientationVectorRadians().Theta, - }, - ) - - executionState := ExecutionState{ - plan: plan, - index: 2, - currentInputs: inputs, - currentPose: map[string]*referenceframe.PoseInFrame{ - localizationFrame.Name(): referenceframe.NewPoseInFrame(referenceframe.World, pathPose), - }, - } - err = CheckPlan(ackermanFrame, localizationFrame, executionState, worldState, tfFrameSystem, 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 10ecd4296f5..ea9920fcee8 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -476,7 +476,7 @@ func NewPoseFrame(name string, geometry spatial.Geometry) (Frame, error) { }, nil } -// Transform on the pose frame acts as the identity function. Whatever inputs are given are directly translated +// 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 { @@ -488,10 +488,10 @@ func (pf *poseFrame) Transform(inputs []Input) (spatial.Pose, error) { // 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), 7) + return nil, NewIncorrectInputLengthError(len(from), len(pf.DoF())) } if err := pf.baseFrame.validInputs(to); err != nil { - return nil, NewIncorrectInputLengthError(len(to), 7) + return nil, NewIncorrectInputLengthError(len(to), len(pf.DoF())) } fromPose, err := pf.Transform(from) if err != nil { diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 3491dacd855..7720730c90c 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -22,6 +22,7 @@ import ( "go.viam.com/rdk/components/gripper" _ "go.viam.com/rdk/components/register" "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" @@ -1128,3 +1129,206 @@ func TestBoundingRegionsConstraint(t *testing.T) { test.That(t, err, test.ShouldBeNil) }) } + +func TestCheckPlan(t *testing.T) { + ctx := context.Background() + logger := logging.NewTestLogger(t) + origin := geo.NewPoint(0, 0) + movementSensor, _, fakeBase, ms := createMoveOnGlobeEnvironment(ctx, t, origin, nil, 5) + defer ms.Close(ctx) + fmt.Println(fakeBase) + dst := geo.NewPoint(origin.Lat(), origin.Lng()+5e-5) + // Note: spatialmath.GeoPointToPoint(dst, origin) produces r3.Vector{5559.746, 0, 0} + + 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) + + fmt.Println(plan.Path()) + fmt.Println(plan.Trajectory()) + wrapperFrame := mr.localizaingFS.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.localizaingFS, 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.localizaingFS, 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.localizaingFS.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.localizaingFS.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.localizaingFS, 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.localizaingFS, 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) + }) + + remainingPlan, err := motionplan.RemainingPlan(plan, 2) + 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: 2727.25}, // X + {Value: 0}, // Y + {Value: 0}, // Z + {Value: 0}, // OX + {Value: 0}, // OY + {Value: 1}, // OZ + {Value: -90}, // 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: 2727.25, Y: 0, Z: 0}, + &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: -90}, + )), + } + + newExecutionState, err := motionplan.NewExecutionState(remainingPlan, 0, 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.localizaingFS, 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.localizaingFS, math.Inf(1), logger) + test.That(t, err, test.ShouldBeNil) + }) +} diff --git a/services/motion/builtin/move_on_map_test.go b/services/motion/builtin/move_on_map_test.go index b10a5c34fa8..c2573e695bb 100644 --- a/services/motion/builtin/move_on_map_test.go +++ b/services/motion/builtin/move_on_map_test.go @@ -589,14 +589,27 @@ func TestMoveOnMapStaticObs(t *testing.T) { }, nil, ) test.That(t, err, test.ShouldBeNil) - currentInputs := referenceframe.StartPositions(mr.localizaingFS) - currentInputs[mr.kinematicBase.LocalizationFrame().Name()] = referenceframe.FloatsToInputs( - []float64{0.58772e3, -0.80826e3, 0, 0, 0, 1, 0}, - ) - executionState, err := motionplan.NewExecutionState( - plan, - 1, - currentInputs, + + 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.720000000000027284841053}, // X + {Value: -808.259999999999990905052982}, // 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.kinematicBase.LocalizationFrame().Name(): referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewPose( r3.Vector{X: 0.58772e3, Y: -0.80826e3, Z: 0}, @@ -604,11 +617,17 @@ func TestMoveOnMapStaticObs(t *testing.T) { )), }, ) + test.That(t, err, test.ShouldBeNil) + + augmentedBaseExecutionState, err := mr.augmentBaseExecutionState(baseExecutionState) + test.That(t, err, test.ShouldBeNil) + + wrapperFrame := mr.localizaingFS.Frame(mr.kinematicBase.Name().Name) + test.That(t, err, test.ShouldBeNil) err = motionplan.CheckPlan( - mr.planRequest.Frame, - mr.kinematicBase.LocalizationFrame(), - executionState, + wrapperFrame, + augmentedBaseExecutionState, wrldSt, mr.localizaingFS, lookAheadDistanceMM, diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 7d8924467a8..583b5b1fed7 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -188,7 +188,6 @@ func (mr *moveRequest) getTransientDetections( if err != nil { return nil, err } - inputMap[mr.kinematicBase.Name().ShortName()] = make([]referenceframe.Input, len(mr.kinematicBase.Kinematics().DoF())) detections, err := visSrvc.GetObjectPointClouds(ctx, camName.Name, nil) if err != nil { @@ -270,30 +269,20 @@ 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, err := mr.createInputMap(ctx, baseExecutionState) - if err != nil { - return state.ExecuteResponse{}, err - } - executionState, err := motionplan.NewExecutionState( - baseExecutionState.Plan(), - baseExecutionState.Index(), - inputMap, - baseExecutionState.CurrentPoses(), - ) + 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.LocalizationFrame().Name()].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 - mr.kinematicBase.LocalizationFrame(), - executionState, + mr.localizaingFS.Frame(mr.kinematicBase.Kinematics().Name()), // frame we wish to check for collisions + updatedBaseExecutionState, worldState, // detected obstacles by this instance of camera + service mr.localizaingFS, lookAheadDistanceMM, @@ -307,6 +296,46 @@ func (mr *moveRequest) obstaclesIntersectPlan( return state.ExecuteResponse{}, nil } +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() { + if idx != 0 { + idx = idx - 1 + } + currPathStep := existingPlan.Path()[idx] + kbPose := currPathStep[mr.kinematicBase.Kinematics().Name()] + kbTraj := currTraj[mr.kinematicBase.Name().Name] + updatedTraj := append(kbTraj, referenceframe.PoseToInputs(kbPose.Pose())...) + 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()] + kinematicBaseCurrentInputs = append( + kinematicBaseCurrentInputs, + referenceframe.PoseToInputs(baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose())..., + ) + allCurrentInputsFromBaseExecutionState[mr.kinematicBase.Kinematics().Name()] = kinematicBaseCurrentInputs + + // update currentPoses + 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 (mr *moveRequest) createInputMap( ctx context.Context, baseExecutionState motionplan.ExecutionState, ) (map[string][]referenceframe.Input, error) { @@ -314,10 +343,11 @@ func (mr *moveRequest) createInputMap( if err != nil { return nil, err } - inputMap[mr.kinematicBase.Name().ShortName()] = baseExecutionState.CurrentInputs()[mr.kinematicBase.Name().Name] - inputMap[mr.kinematicBase.LocalizationFrame().Name()] = referenceframe.PoseToInputs( + 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 return inputMap, nil } @@ -707,7 +737,7 @@ func (ms *builtIn) createBaseMoveRequest( // create fs used for planning planningFS := referenceframe.NewEmptyFrameSystem("store-starting point") - transformFrame, err := referenceframe.NewStaticFrame(kb.Kinematics().Name()+"LocalizationFrame", startPose) + transformFrame, err := referenceframe.NewStaticFrame(kb.LocalizationFrame().Name(), startPose) if err != nil { return nil, err } @@ -720,13 +750,18 @@ func (ms *builtIn) createBaseMoveRequest( return nil, err } - // create collision checking fs - localizingFS := referenceframe.NewEmptyFrameSystem("localizingFS") - err = localizingFS.AddFrame(kb.LocalizationFrame(), localizingFS.World()) - if err != nil { - return nil, err - } - err = localizingFS.MergeFrameSystem(baseOnlyFS, kb.LocalizationFrame()) + // create wrapperFrame and fs used for collision checks + executionFrame := kb.Kinematics() + localizationFrame := kb.LocalizationFrame() + wrapperFS := referenceframe.NewEmptyFrameSystem("wrapperFS") + wrapperFS.AddFrame(localizationFrame, wrapperFS.World()) + wrapperFS.AddFrame(executionFrame, localizationFrame) + wrapperFrameSeedMap := map[string][]referenceframe.Input{} + wrapperFrameSeedMap[executionFrame.Name()] = make([]referenceframe.Input, len(executionFrame.DoF())) + wrapperFrameSeedMap[localizationFrame.Name()] = make([]referenceframe.Input, len(localizationFrame.DoF())) + wf := NewWrapperFrame(localizationFrame, executionFrame, wrapperFrameSeedMap, wrapperFS) + collisionFS := baseOnlyFS + err = collisionFS.ReplaceFrame(wf) if err != nil { return nil, err } @@ -807,7 +842,7 @@ func (ms *builtIn) createBaseMoveRequest( replanCostFactor: valExtra.replanCostFactor, obstacleDetectors: obstacleDetectors, fsService: ms.fsService, - localizaingFS: localizingFS, + localizaingFS: collisionFS, executeBackgroundWorkers: &backgroundWorkers, diff --git a/services/motion/builtin/wrapperFrame.go b/services/motion/builtin/wrapperFrame.go new file mode 100644 index 00000000000..9058fde8cc0 --- /dev/null +++ b/services/motion/builtin/wrapperFrame.go @@ -0,0 +1,158 @@ +package builtin + +import ( + "github.com/pkg/errors" + "go.uber.org/multierr" + pb "go.viam.com/api/component/arm/v1" + "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. +// This struct is used so that we do not break abstractions made in CheckPlan. +type wrapperFrame struct { + name string + localizationFrame referenceframe.Frame + executionFrame referenceframe.Frame + seedMap map[string][]referenceframe.Input + fs referenceframe.FrameSystem +} + +func NewWrapperFrame( + localizationFrame, executionFrame referenceframe.Frame, + seedMap map[string][]referenceframe.Input, + fs referenceframe.FrameSystem, +) referenceframe.Frame { + return &wrapperFrame{ + name: executionFrame.Name(), + localizationFrame: localizationFrame, + executionFrame: executionFrame, + seedMap: seedMap, + fs: fs, + } +} + +// 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())) + } + pf := referenceframe.NewPoseInFrame(wf.Name(), spatialmath.NewZeroPose()) + tf, err := wf.fs.Transform(wf.sliceToMap(inputs), pf, referenceframe.World) + if err != nil { + return nil, err + } + return tf.(*referenceframe.PoseInFrame).Pose(), 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)) + + // executionFrame interpolation + fromSubset := from[:len(wf.executionFrame.DoF())] + toSubset := to[:len(wf.executionFrame.DoF())] + interpSub, err := wf.executionFrame.Interpolate(fromSubset, toSubset, by) + if err != nil { + return nil, err + } + interp = append(interp, interpSub...) + + // localizationFrame interpolation + fromSubset = from[len(wf.executionFrame.DoF()):] + toSubset = to[len(wf.executionFrame.DoF()):] + interpSub, err = wf.localizationFrame.Interpolate(fromSubset, toSubset, by) + if err != nil { + return nil, err + } + interp = append(interp, interpSub...) + + 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())) + } + var errAll error + inputMap := wf.sliceToMap(inputs) + sfGeometries := []spatialmath.Geometry{} + for _, fName := range wf.fs.FrameNames() { + f := wf.fs.Frame(fName) + if f == nil { + return nil, referenceframe.NewFrameMissingError(fName) + } + inputs, err := referenceframe.GetFrameInputs(f, inputMap) + if err != nil { + return nil, err + } + gf, err := f.Geometries(inputs) + if gf == nil { + // only propagate errors that result in nil geometry + multierr.AppendInto(&errAll, err) + continue + } + var tf referenceframe.Transformable + tf, err = wf.fs.Transform(inputMap, gf, referenceframe.World) + if err != nil { + return nil, err + } + sfGeometries = append(sfGeometries, tf.(*referenceframe.GeometriesInFrame).Geometries()...) + } + return referenceframe.NewGeometriesInFrame(referenceframe.World, sfGeometries), errAll +} + +// DoF returns the number of degrees of freedom within a given frame. +func (wf *wrapperFrame) DoF() []referenceframe.Limit { + var limits []referenceframe.Limit + for _, name := range wf.fs.FrameNames() { + limits = append(limits, wf.fs.Frame(name).DoF()...) + } + return limits +} + +// 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 { + n := make([]referenceframe.Input, len(jp.Values)) + for idx, d := range jp.Values { + n[idx] = referenceframe.Input{Value: utils.DegToRad(d)} + } + return n +} + +// ProtobufFromInput converts inputs to pb.JointPosition. +func (wf *wrapperFrame) ProtobufFromInput(input []referenceframe.Input) *pb.JointPositions { + n := make([]float64, len(input)) + for idx, a := range input { + n[idx] = utils.RadToDeg(a.Value) + } + return &pb.JointPositions{Values: n} +} + +func (wf *wrapperFrame) sliceToMap(inputSlice []referenceframe.Input) map[string][]referenceframe.Input { + inputs := map[string][]referenceframe.Input{} + for k, v := range wf.seedMap { + inputs[k] = v + } + inputs[wf.executionFrame.Name()] = inputSlice[:len(wf.executionFrame.DoF())] + inputs[wf.localizationFrame.Name()] = inputSlice[len(wf.executionFrame.DoF()):] + return inputs +} diff --git a/services/motion/explore/explore.go b/services/motion/explore/explore.go index 777843e8c8b..f5dacc5ebe0 100644 --- a/services/motion/explore/explore.go +++ b/services/motion/explore/explore.go @@ -379,7 +379,6 @@ func (ms *explore) checkForObstacles( // Check plan for transient obstacles err = motionplan.CheckPlan( kb.Kinematics(), - nil, executionState, worldState, ms.frameSystem, From 85ad5b5bed939d54a5894d1f96286026bff67ad0 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Tue, 25 Jun 2024 14:24:35 -0400 Subject: [PATCH 079/126] linted --- motionplan/check.go | 12 ++++---- referenceframe/frame.go | 2 +- services/motion/builtin/builtin_test.go | 5 ++-- services/motion/builtin/move_request.go | 39 ++++++++++++------------- services/motion/builtin/wrapperFrame.go | 3 +- 5 files changed, 29 insertions(+), 32 deletions(-) diff --git a/motionplan/check.go b/motionplan/check.go index 61bbb5c3cdc..eca4e701741 100644 --- a/motionplan/check.go +++ b/motionplan/check.go @@ -15,7 +15,7 @@ import ( ) var ( - checkFrameNotInPathErr = errors.New("checkFrame given not in plan.Path() map") + errCheckFrameNotInPath = errors.New("checkFrame given not in plan.Path() map") ptgDoFLen = 4 ) @@ -126,7 +126,7 @@ func checkPlanRelative( // determine plan's starting pose planStartPiF, ok := plan.Path()[0][checkFrame.Name()] if !ok { - return checkFrameNotInPathErr // THIS ERROR APPEARS TWICE, SO WE CAN MAKE IT INTO A VAR + return errCheckFrameNotInPath // THIS ERROR APPEARS TWICE, SO WE CAN MAKE IT INTO A VAR } planStartPoseWorld, err := toWorld(planStartPiF, plan.Trajectory()[0]) if err != nil { @@ -136,7 +136,7 @@ func checkPlanRelative( // determine plan's ending pose planEndPiF, ok := plan.Path()[len(plan.Path())-1][checkFrame.Name()] if !ok { - return checkFrameNotInPathErr + return errCheckFrameNotInPath } planEndPoseWorld, err := toWorld(planEndPiF, plan.Trajectory()[len(plan.Path())-1]) if err != nil { @@ -194,7 +194,7 @@ func checkPlanRelative( // Check that path pose is valid stepEndPiF, ok := plan.Path()[wayPointIdx][checkFrame.Name()] if !ok { - return checkFrameNotInPathErr + return errCheckFrameNotInPath } expectedArcEndInWorld, err := toWorld(stepEndPiF, plan.Trajectory()[wayPointIdx]) @@ -204,7 +204,7 @@ func checkPlanRelative( arcInputs, ok := plan.Trajectory()[wayPointIdx][checkFrame.Name()] if !ok { - return checkFrameNotInPathErr + return errCheckFrameNotInPath } fullArcPose, err := checkFrame.Transform(arcInputs) if err != nil { @@ -242,7 +242,7 @@ func checkPlanRelative( for i := wayPointIdx + 1; i <= len(plan.Path())-1; i++ { thisArcEndPoseTf, ok := plan.Path()[i][checkFrame.Name()] if !ok { - return checkFrameNotInPathErr + return errCheckFrameNotInPath } thisArcEndPoseInWorld, err := toWorld(thisArcEndPoseTf, plan.Trajectory()[i]) if err != nil { diff --git a/referenceframe/frame.go b/referenceframe/frame.go index ea9920fcee8..558773d0ac7 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -558,7 +558,7 @@ func PoseToInputs(p spatial.Pose) []Input { }) } -// InputsToPose is a convience method for turning inputs into a spatial.Pose +// InputsToPose is a convience method for turning inputs into a spatial.Pose. func InputsToPose(inputs []Input) spatial.Pose { return spatial.NewPose( r3.Vector{X: inputs[0].Value, Y: inputs[1].Value, Z: inputs[2].Value}, diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 7720730c90c..8f641a0d31b 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -1134,9 +1134,10 @@ func TestCheckPlan(t *testing.T) { ctx := context.Background() logger := logging.NewTestLogger(t) origin := geo.NewPoint(0, 0) + movementSensor, _, fakeBase, ms := createMoveOnGlobeEnvironment(ctx, t, origin, nil, 5) defer ms.Close(ctx) - fmt.Println(fakeBase) + dst := geo.NewPoint(origin.Lat(), origin.Lng()+5e-5) // Note: spatialmath.GeoPointToPoint(dst, origin) produces r3.Vector{5559.746, 0, 0} @@ -1157,8 +1158,6 @@ func TestCheckPlan(t *testing.T) { test.That(t, err, test.ShouldBeNil) test.That(t, len(plan.Path()), test.ShouldBeGreaterThan, 2) - fmt.Println(plan.Path()) - fmt.Println(plan.Trajectory()) wrapperFrame := mr.localizaingFS.Frame(mr.kinematicBase.Name().Name) currentInputs := map[string][]referenceframe.Input{ diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 583b5b1fed7..931d32c8ce2 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -184,10 +184,15 @@ func (mr *moveRequest) getTransientDetections( if err != nil { return nil, err } - inputMap, err := mr.createInputMap(ctx, baseExecutionState) + 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 detections, err := visSrvc.GetObjectPointClouds(ctx, camName.Name, nil) if err != nil { @@ -304,12 +309,13 @@ func (mr *moveRequest) augmentBaseExecutionState( newTrajectory := make(motionplan.Trajectory, 0, len(existingPlan.Trajectory())) for idx, currTraj := range existingPlan.Trajectory() { if idx != 0 { - idx = idx - 1 + idx-- } currPathStep := existingPlan.Path()[idx] kbPose := currPathStep[mr.kinematicBase.Kinematics().Name()] kbTraj := currTraj[mr.kinematicBase.Name().Name] - updatedTraj := append(kbTraj, referenceframe.PoseToInputs(kbPose.Pose())...) + updatedTraj := kbTraj + updatedTraj = append(updatedTraj, referenceframe.PoseToInputs(kbPose.Pose())...) newTrajectory = append( newTrajectory, map[string][]referenceframe.Input{mr.kinematicBase.Kinematics().Name(): updatedTraj}, ) @@ -336,21 +342,6 @@ func (mr *moveRequest) augmentBaseExecutionState( ) } -func (mr *moveRequest) createInputMap( - ctx context.Context, baseExecutionState motionplan.ExecutionState, -) (map[string][]referenceframe.Input, error) { - 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 - return inputMap, nil -} - func kbOptionsFromCfg(motionCfg *validatedMotionConfiguration, validatedExtra validatedExtra) kinematicbase.Options { kinematicsOptions := kinematicbase.NewKinematicBaseOptions() @@ -754,12 +745,18 @@ func (ms *builtIn) createBaseMoveRequest( executionFrame := kb.Kinematics() localizationFrame := kb.LocalizationFrame() wrapperFS := referenceframe.NewEmptyFrameSystem("wrapperFS") - wrapperFS.AddFrame(localizationFrame, wrapperFS.World()) - wrapperFS.AddFrame(executionFrame, localizationFrame) + err = wrapperFS.AddFrame(localizationFrame, wrapperFS.World()) + if err != nil { + return nil, err + } + err = wrapperFS.AddFrame(executionFrame, localizationFrame) + if err != nil { + return nil, err + } wrapperFrameSeedMap := map[string][]referenceframe.Input{} wrapperFrameSeedMap[executionFrame.Name()] = make([]referenceframe.Input, len(executionFrame.DoF())) wrapperFrameSeedMap[localizationFrame.Name()] = make([]referenceframe.Input, len(localizationFrame.DoF())) - wf := NewWrapperFrame(localizationFrame, executionFrame, wrapperFrameSeedMap, wrapperFS) + wf := newWrapperFrame(localizationFrame, executionFrame, wrapperFrameSeedMap, wrapperFS) collisionFS := baseOnlyFS err = collisionFS.ReplaceFrame(wf) if err != nil { diff --git a/services/motion/builtin/wrapperFrame.go b/services/motion/builtin/wrapperFrame.go index 9058fde8cc0..e6864ff8897 100644 --- a/services/motion/builtin/wrapperFrame.go +++ b/services/motion/builtin/wrapperFrame.go @@ -4,6 +4,7 @@ import ( "github.com/pkg/errors" "go.uber.org/multierr" pb "go.viam.com/api/component/arm/v1" + "go.viam.com/rdk/referenceframe" "go.viam.com/rdk/spatialmath" "go.viam.com/rdk/utils" @@ -19,7 +20,7 @@ type wrapperFrame struct { fs referenceframe.FrameSystem } -func NewWrapperFrame( +func newWrapperFrame( localizationFrame, executionFrame referenceframe.Frame, seedMap map[string][]referenceframe.Input, fs referenceframe.FrameSystem, From d8b7771005fba9ab2ac2f1e270e14c226511140b Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 3 Jul 2024 11:24:08 -0400 Subject: [PATCH 080/126] merge with main --- services/motion/builtin/builtin_test.go | 11 +- services/motion/builtin/builtin_utils_test.go | 306 ------------------ services/motion/builtin/move_on_map_test.go | 1 + 3 files changed, 9 insertions(+), 309 deletions(-) delete mode 100644 services/motion/builtin/builtin_utils_test.go diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 5b6315e0bc6..603b14c14ab 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -910,7 +910,6 @@ func TestBaseInputs(t *testing.T) { defer closeFunc(ctx) 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) { @@ -918,12 +917,18 @@ func TestCheckPlan(t *testing.T) { logger := logging.NewTestLogger(t) origin := geo.NewPoint(0, 0) - movementSensor, _, fakeBase, ms := createMoveOnGlobeEnvironment(ctx, t, origin, nil, 5) - defer ms.Close(ctx) + 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, diff --git a/services/motion/builtin/builtin_utils_test.go b/services/motion/builtin/builtin_utils_test.go deleted file mode 100644 index 137de3466ca..00000000000 --- a/services/motion/builtin/builtin_utils_test.go +++ /dev/null @@ -1,306 +0,0 @@ -package builtin - -import ( - "context" - "os" - "path/filepath" - "testing" - - "github.com/golang/geo/r3" - geo "github.com/kellydunn/golang-geo" - "go.viam.com/test" - "go.viam.com/utils" - "go.viam.com/utils/artifact" - - "go.viam.com/rdk/components/base" - baseFake "go.viam.com/rdk/components/base/fake" - "go.viam.com/rdk/components/base/kinematicbase" - "go.viam.com/rdk/components/movementsensor" - "go.viam.com/rdk/config" - "go.viam.com/rdk/logging" - "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/resource" - "go.viam.com/rdk/robot/framesystem" - robotimpl "go.viam.com/rdk/robot/impl" - "go.viam.com/rdk/services/motion" - "go.viam.com/rdk/services/slam" - "go.viam.com/rdk/spatialmath" - "go.viam.com/rdk/testutils/inject" -) - -func setupMotionServiceFromConfig(t *testing.T, configFilename string) (motion.Service, func()) { - t.Helper() - ctx := context.Background() - logger := logging.NewTestLogger(t) - cfg, err := config.Read(ctx, configFilename, logger) - test.That(t, err, test.ShouldBeNil) - myRobot, err := robotimpl.New(ctx, cfg, logger) - test.That(t, err, test.ShouldBeNil) - svc, err := motion.FromRobot(myRobot, "builtin") - test.That(t, err, test.ShouldBeNil) - return svc, func() { - myRobot.Close(context.Background()) - } -} - -func getPointCloudMap(path string) (func() ([]byte, error), error) { - const chunkSizeBytes = 1 * 1024 * 1024 - file, err := os.Open(path) - if err != nil { - return nil, err - } - chunk := make([]byte, chunkSizeBytes) - f := func() ([]byte, error) { - bytesRead, err := file.Read(chunk) - if err != nil { - defer utils.UncheckedErrorFunc(file.Close) - return nil, err - } - return chunk[:bytesRead], err - } - return f, nil -} - -func createInjectedMovementSensor(name string, gpsPoint *geo.Point) *inject.MovementSensor { - injectedMovementSensor := inject.NewMovementSensor(name) - injectedMovementSensor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { - return gpsPoint, 0, nil - } - injectedMovementSensor.CompassHeadingFunc = func(ctx context.Context, extra map[string]interface{}) (float64, error) { - return 0, nil - } - injectedMovementSensor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { - return &movementsensor.Properties{CompassHeadingSupported: true}, nil - } - - return injectedMovementSensor -} - -func createInjectedSlam(name, pcdPath string, origin spatialmath.Pose) *inject.SLAMService { - injectSlam := inject.NewSLAMService(name) - injectSlam.PointCloudMapFunc = func(ctx context.Context, returnEditedMap bool) (func() ([]byte, error), error) { - return getPointCloudMap(filepath.Clean(artifact.MustPath(pcdPath))) - } - injectSlam.PositionFunc = func(ctx context.Context) (spatialmath.Pose, error) { - if origin != nil { - return origin, nil - } - return spatialmath.NewZeroPose(), nil - } - injectSlam.PropertiesFunc = func(ctx context.Context) (slam.Properties, error) { - return slam.Properties{ - CloudSlam: false, - MappingMode: slam.MappingModeLocalizationOnly, - InternalStateFileType: ".pbstream", - SensorInfo: []slam.SensorInfo{ - {Name: "my-camera", Type: slam.SensorTypeCamera}, - {Name: "my-movement-sensor", Type: slam.SensorTypeMovementSensor}, - }, - }, nil - } - return injectSlam -} - -func createBaseLink(t *testing.T) *referenceframe.LinkInFrame { - basePose := spatialmath.NewPoseFromPoint(r3.Vector{X: 0, Y: 0, Z: 0}) - baseSphere, err := spatialmath.NewSphere(basePose, 10, "base-sphere") - test.That(t, err, test.ShouldBeNil) - baseLink := referenceframe.NewLinkInFrame( - referenceframe.World, - spatialmath.NewZeroPose(), - "test-base", - baseSphere, - ) - return baseLink -} - -func createFrameSystemService( - ctx context.Context, - deps resource.Dependencies, - fsParts []*referenceframe.FrameSystemPart, - logger logging.Logger, -) (framesystem.Service, error) { - fsSvc, err := framesystem.New(ctx, deps, logger) - if err != nil { - return nil, err - } - conf := resource.Config{ - ConvertedAttributes: &framesystem.Config{Parts: fsParts}, - } - if err := fsSvc.Reconfigure(ctx, deps, conf); err != nil { - return nil, err - } - deps[fsSvc.Name()] = fsSvc - - return fsSvc, nil -} - -func createMoveOnGlobeEnvironment(ctx context.Context, t *testing.T, origin *geo.Point, noise spatialmath.Pose, sleepTime int) ( - *inject.MovementSensor, framesystem.Service, kinematicbase.KinematicBase, motion.Service, -) { - logger := logging.NewTestLogger(t) - - // create fake base - baseCfg := resource.Config{ - Name: "test-base", - API: base.API, - Frame: &referenceframe.LinkConfig{Geometry: &spatialmath.GeometryConfig{R: 80}}, - } - fakeBase, err := baseFake.NewBase(ctx, nil, baseCfg, logger) - test.That(t, err, test.ShouldBeNil) - - // create base link - baseLink := createBaseLink(t) - // create MovementSensor link - movementSensorLink := referenceframe.NewLinkInFrame( - baseLink.Name(), - spatialmath.NewPoseFromPoint(r3.Vector{X: -10, Y: 0, Z: 0}), - "test-gps", - nil, - ) - - // create a fake kinematic base - kinematicsOptions := kinematicbase.NewKinematicBaseOptions() - kinematicsOptions.PlanDeviationThresholdMM = 1 // can afford to do this for tests - kb, err := kinematicbase.WrapWithFakePTGKinematics( - ctx, - fakeBase.(*baseFake.Base), - logger, - referenceframe.NewPoseInFrame(referenceframe.World, spatialmath.NewZeroPose()), - kinematicsOptions, - noise, - sleepTime, - ) - test.That(t, err, test.ShouldBeNil) - - // create injected MovementSensor - dynamicMovementSensor := inject.NewMovementSensor("test-gps") - dynamicMovementSensor.PositionFunc = func(ctx context.Context, extra map[string]interface{}) (*geo.Point, float64, error) { - poseInFrame, err := kb.CurrentPosition(ctx) - test.That(t, err, test.ShouldBeNil) - heading := poseInFrame.Pose().Orientation().OrientationVectorDegrees().Theta + 180 - distance := poseInFrame.Pose().Point().Norm() - pt := origin.PointAtDistanceAndBearing(distance*1e-6, heading) - return pt, 0, nil - } - dynamicMovementSensor.CompassHeadingFunc = func(ctx context.Context, extra map[string]interface{}) (float64, error) { - return 0, nil - } - dynamicMovementSensor.PropertiesFunc = func(ctx context.Context, extra map[string]interface{}) (*movementsensor.Properties, error) { - return &movementsensor.Properties{CompassHeadingSupported: true}, nil - } - - // create injected vision service - injectedVisionSvc := inject.NewVisionService("injectedVisionSvc") - - cameraGeom, err := spatialmath.NewBox( - spatialmath.NewZeroPose(), - r3.Vector{X: 1, Y: 1, Z: 1}, "camera", - ) - test.That(t, err, test.ShouldBeNil) - - injectedCamera := inject.NewCamera("injectedCamera") - cameraLink := referenceframe.NewLinkInFrame( - baseLink.Name(), - spatialmath.NewPose(r3.Vector{X: 1}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: 90}), - "injectedCamera", - cameraGeom, - ) - - // create the frame system - fsParts := []*referenceframe.FrameSystemPart{ - {FrameConfig: movementSensorLink}, - {FrameConfig: baseLink}, - {FrameConfig: cameraLink}, - } - deps := resource.Dependencies{ - fakeBase.Name(): kb, - dynamicMovementSensor.Name(): dynamicMovementSensor, - injectedVisionSvc.Name(): injectedVisionSvc, - injectedCamera.Name(): injectedCamera, - } - - fsSvc, err := createFrameSystemService(ctx, deps, fsParts, logger) - test.That(t, err, test.ShouldBeNil) - - conf := resource.Config{ConvertedAttributes: &Config{}} - ms, err := NewBuiltIn(ctx, deps, conf, logger) - test.That(t, err, test.ShouldBeNil) - - return dynamicMovementSensor, fsSvc, kb, ms -} - -func createMoveOnMapEnvironment( - ctx context.Context, - t *testing.T, - pcdPath string, - geomSize float64, - origin spatialmath.Pose, -) (kinematicbase.KinematicBase, motion.Service) { - if origin == nil { - origin = spatialmath.NewZeroPose() - } - injectSlam := createInjectedSlam("test_slam", pcdPath, origin) - injectVis := inject.NewVisionService("test-vision") - injectCam := inject.NewCamera("test-camera") - - baseLink := createBaseLink(t) - - cfg := resource.Config{ - Name: "test-base", - API: base.API, - Frame: &referenceframe.LinkConfig{Geometry: &spatialmath.GeometryConfig{R: geomSize, Label: "kinematicBaseGeometry"}}, - } - logger := logging.NewTestLogger(t) - fakeBase, err := baseFake.NewBase(ctx, nil, cfg, logger) - test.That(t, err, test.ShouldBeNil) - kinematicsOptions := kinematicbase.NewKinematicBaseOptions() - kinematicsOptions.PlanDeviationThresholdMM = 1 // can afford to do this for tests - kb, err := kinematicbase.WrapWithFakePTGKinematics( - ctx, - fakeBase.(*baseFake.Base), - logger, - referenceframe.NewPoseInFrame(referenceframe.World, origin), - kinematicsOptions, - spatialmath.NewZeroPose(), - 10, - ) - test.That(t, err, test.ShouldBeNil) - - deps := resource.Dependencies{ - fakeBase.Name(): kb, - injectVis.Name(): injectVis, - injectCam.Name(): injectCam, - injectSlam.Name(): injectSlam, - } - conf := resource.Config{ConvertedAttributes: &Config{}} - - ms, err := NewBuiltIn(ctx, deps, conf, logger) - test.That(t, err, test.ShouldBeNil) - - // create the frame system - cameraGeom, err := spatialmath.NewBox( - spatialmath.NewZeroPose(), - r3.Vector{X: 1, Y: 1, Z: 1}, "camera", - ) - test.That(t, err, test.ShouldBeNil) - cameraLink := referenceframe.NewLinkInFrame( - baseLink.Name(), - // 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{X: 0, Y: 0, Z: 0}, &spatialmath.OrientationVectorDegrees{OY: 1, Theta: -90}), - "test-camera", - cameraGeom, - ) - - fsParts := []*referenceframe.FrameSystemPart{ - {FrameConfig: baseLink}, - {FrameConfig: cameraLink}, - } - - fsSvc, err := createFrameSystemService(ctx, deps, fsParts, logger) - test.That(t, err, test.ShouldBeNil) - ms.(*builtIn).fsService = fsSvc - - return kb, ms -} diff --git a/services/motion/builtin/move_on_map_test.go b/services/motion/builtin/move_on_map_test.go index 0cdf8f4f653..e24c792d67e 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" From b1f845e86c148f5ccc715834160ac4114b874089 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 3 Jul 2024 11:27:48 -0400 Subject: [PATCH 081/126] remove fake_kinematics - somehow not caught when I merged with main --- .../base/kinematicbase/fake_kinematics.go | 389 ------------------ 1 file changed, 389 deletions(-) delete mode 100644 components/base/kinematicbase/fake_kinematics.go diff --git a/components/base/kinematicbase/fake_kinematics.go b/components/base/kinematicbase/fake_kinematics.go deleted file mode 100644 index 61e0d79eb21..00000000000 --- a/components/base/kinematicbase/fake_kinematics.go +++ /dev/null @@ -1,389 +0,0 @@ -//go:build !no_cgo - -package kinematicbase - -import ( - "context" - "errors" - "sync" - "time" - - "go.viam.com/rdk/components/base/fake" - "go.viam.com/rdk/logging" - "go.viam.com/rdk/motionplan" - "go.viam.com/rdk/motionplan/tpspace" - "go.viam.com/rdk/referenceframe" - "go.viam.com/rdk/services/motion" - "go.viam.com/rdk/spatialmath" - rdkutils "go.viam.com/rdk/utils" -) - -type fakeDiffDriveKinematics struct { - *fake.Base - parentFrame string - planningFrame, localizationFrame referenceframe.Frame - inputs []referenceframe.Input - options Options - sensorNoise spatialmath.Pose - lock sync.RWMutex -} - -// WrapWithFakeDiffDriveKinematics creates a DiffDrive KinematicBase from the fake Base so that it satisfies the ModelFramer and -// InputEnabled interfaces. -func WrapWithFakeDiffDriveKinematics( - ctx context.Context, - b *fake.Base, - localizer motion.Localizer, - limits []referenceframe.Limit, - options Options, - sensorNoise spatialmath.Pose, -) (KinematicBase, error) { - position, err := localizer.CurrentPosition(ctx) - if err != nil { - return nil, err - } - pt := position.Pose().Point() - if sensorNoise == nil { - sensorNoise = spatialmath.NewZeroPose() - } - fk := &fakeDiffDriveKinematics{ - Base: b, - parentFrame: position.Parent(), - inputs: referenceframe.FloatsToInputs([]float64{pt.X, pt.Y}), - sensorNoise: sensorNoise, - } - var geometry spatialmath.Geometry - if len(fk.Base.Geometry) != 0 { - geometry = fk.Base.Geometry[0] - } - - fk.localizationFrame, err = referenceframe.New2DMobileModelFrame(b.Name().ShortName(), limits, geometry) - if err != nil { - return nil, err - } - - if options.PositionOnlyMode { - fk.planningFrame, err = referenceframe.New2DMobileModelFrame(b.Name().ShortName(), limits[:2], geometry) - if err != nil { - return nil, err - } - } else { - fk.planningFrame = fk.localizationFrame - } - - fk.options = options - return fk, nil -} - -func (fk *fakeDiffDriveKinematics) Kinematics() referenceframe.Frame { - return fk.planningFrame -} - -func (fk *fakeDiffDriveKinematics) LocalizationFrame() referenceframe.Frame { - return fk.localizationFrame -} - -func (fk *fakeDiffDriveKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { - fk.lock.RLock() - defer fk.lock.RUnlock() - return fk.inputs, nil -} - -func (fk *fakeDiffDriveKinematics) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Input) error { - for _, inputs := range inputSteps { - _, err := fk.planningFrame.Transform(inputs) - if err != nil { - return err - } - fk.lock.Lock() - fk.inputs = inputs - fk.lock.Unlock() - - // Sleep for a short amount to time to simulate a base taking some amount of time to reach the inputs - time.Sleep(150 * time.Millisecond) - } - return nil -} - -func (fk *fakeDiffDriveKinematics) ErrorState(ctx context.Context) (spatialmath.Pose, error) { - return fk.sensorNoise, nil -} - -func (fk *fakeDiffDriveKinematics) ExecutionState(ctx context.Context) (motionplan.ExecutionState, error) { - return motionplan.ExecutionState{}, errors.New("fakeDiffDriveKinematics does not support executionState") -} - -func (fk *fakeDiffDriveKinematics) CurrentPosition(ctx context.Context) (*referenceframe.PoseInFrame, error) { - fk.lock.RLock() - inputs := fk.inputs - fk.lock.RUnlock() - currentPose, err := fk.planningFrame.Transform(inputs) - if err != nil { - return nil, err - } - return referenceframe.NewPoseInFrame(fk.parentFrame, spatialmath.Compose(currentPose, fk.sensorNoise)), nil -} - -type fakePTGKinematics struct { - *fake.Base - localizer motion.Localizer - planningFrame, localizationFrame referenceframe.Frame - options Options - sensorNoise spatialmath.Pose - ptgs []tpspace.PTGSolver - currentInput []referenceframe.Input - currentIndex int - plan motionplan.Plan - origin *referenceframe.PoseInFrame - positionlock sync.RWMutex - inputLock sync.RWMutex - logger logging.Logger - sleepTime int -} - -// WrapWithFakePTGKinematics creates a PTG KinematicBase from the fake Base so that it satisfies the ModelFramer and InputEnabled -// interfaces. -func WrapWithFakePTGKinematics( - ctx context.Context, - b *fake.Base, - logger logging.Logger, - origin *referenceframe.PoseInFrame, - options Options, - sensorNoise spatialmath.Pose, - sleepTime int, -) (KinematicBase, error) { - properties, err := b.Properties(ctx, nil) - if err != nil { - return nil, err - } - - baseMillimetersPerSecond := options.LinearVelocityMMPerSec - if baseMillimetersPerSecond == 0 { - baseMillimetersPerSecond = defaultLinearVelocityMMPerSec - } - - baseTurningRadiusMeters := properties.TurningRadiusMeters - if baseTurningRadiusMeters < 0 { - return nil, errors.New("can only wrap with PTG kinematics if turning radius is greater than or equal to zero") - } - - angVelocityDegsPerSecond, err := correctAngularVelocityWithTurnRadius( - logger, - baseTurningRadiusMeters, - baseMillimetersPerSecond, - options.AngularVelocityDegsPerSec, - ) - if err != nil { - return nil, err - } - - geometries, err := b.Geometries(ctx, nil) - if err != nil { - return nil, err - } - - nonzeroBaseTurningRadiusMeters := (baseMillimetersPerSecond / rdkutils.DegToRad(angVelocityDegsPerSecond)) / 1000. - - // construct planning frame - planningFrame, err := tpspace.NewPTGFrameFromKinematicOptions( - b.Name().ShortName(), - logger, - nonzeroBaseTurningRadiusMeters, - 0, // If zero, will use default on the receiver end. - geometries, - options.NoSkidSteer, - baseTurningRadiusMeters == 0, - ) - if err != nil { - return nil, err - } - - // construct localization frame - localizationFrame, err := referenceframe.NewPoseFrame(b.Name().ShortName()+"LocalizationFrame", nil) - if err != nil { - return nil, err - } - - if sensorNoise == nil { - sensorNoise = spatialmath.NewZeroPose() - } - - ptgProv, ok := planningFrame.(tpspace.PTGProvider) - if !ok { - return nil, errors.New("unable to cast ptgk frame to a PTG Provider") - } - ptgs := ptgProv.PTGSolvers() - traj := motionplan.Trajectory{{planningFrame.Name(): zeroInput}} - path := motionplan.Path{ - {planningFrame.Name(): referenceframe.NewPoseInFrame(origin.Parent(), spatialmath.Compose(origin.Pose(), sensorNoise))}, - } - zeroPlan := motionplan.NewSimplePlan(path, traj) - - fk := &fakePTGKinematics{ - Base: b, - planningFrame: planningFrame, - localizationFrame: localizationFrame, - origin: origin, - ptgs: ptgs, - currentInput: zeroInput, - currentIndex: 0, - plan: zeroPlan, - sensorNoise: sensorNoise, - logger: logger, - sleepTime: sleepTime, - } - initLocalizer := &fakePTGKinematicsLocalizer{fk} - fk.localizer = motion.TwoDLocalizer(initLocalizer) - - fk.options = options - return fk, nil -} - -func (fk *fakePTGKinematics) Kinematics() referenceframe.Frame { - return fk.planningFrame -} - -func (fk *fakePTGKinematics) LocalizationFrame() referenceframe.Frame { - return fk.localizationFrame -} - -func (fk *fakePTGKinematics) CurrentInputs(ctx context.Context) ([]referenceframe.Input, error) { - fk.inputLock.RLock() - defer fk.inputLock.RUnlock() - return fk.currentInput, nil -} - -func (fk *fakePTGKinematics) GoToInputs(ctx context.Context, inputSteps ...[]referenceframe.Input) error { - defer func() { - fk.inputLock.Lock() - fk.currentInput = zeroInput - fk.currentIndex = 0 - - traj := motionplan.Trajectory{{fk.planningFrame.Name(): zeroInput}} - path := motionplan.Path{ - {fk.planningFrame.Name(): referenceframe.NewPoseInFrame(fk.origin.Parent(), spatialmath.Compose(fk.origin.Pose(), fk.sensorNoise))}, - } - fk.plan = motionplan.NewSimplePlan(path, traj) - fk.inputLock.Unlock() - }() - - currPos, err := fk.CurrentPosition(ctx) - if err != nil { - return err - } - - fk.inputLock.Lock() - fk.plan, err = inputsToPlan(inputSteps, currPos, fk.Kinematics()) - fk.inputLock.Unlock() - if err != nil { - return err - } - - for i, inputs := range inputSteps { - fk.positionlock.RLock() - startingPose := fk.origin - fk.positionlock.RUnlock() - - fk.inputLock.Lock() - fk.currentIndex = i - fk.currentInput, err = fk.planningFrame.Interpolate(zeroInput, inputs, 0) - fk.inputLock.Unlock() - if err != nil { - return err - } - finalPose, err := fk.planningFrame.Transform(inputs) - if err != nil { - return err - } - - steps := motionplan.PathStepCount(spatialmath.NewZeroPose(), finalPose, 2) - var interpolatedConfigurations [][]referenceframe.Input - for i := 0; i <= steps; i++ { - interp := float64(i) / float64(steps) - interpConfig, err := fk.planningFrame.Interpolate(zeroInput, inputs, interp) - if err != nil { - return err - } - interpolatedConfigurations = append(interpolatedConfigurations, interpConfig) - } - for _, inter := range interpolatedConfigurations { - if ctx.Err() != nil { - return ctx.Err() - } - relativePose, err := fk.planningFrame.Transform(inter) - if err != nil { - return err - } - newPose := spatialmath.Compose(startingPose.Pose(), relativePose) - - fk.positionlock.Lock() - fk.origin = referenceframe.NewPoseInFrame(fk.origin.Parent(), newPose) - fk.positionlock.Unlock() - - fk.inputLock.Lock() - fk.currentInput = inter - fk.inputLock.Unlock() - - time.Sleep(time.Duration(fk.sleepTime) * time.Microsecond * 10) - } - } - return nil -} - -func (fk *fakePTGKinematics) ErrorState(ctx context.Context) (spatialmath.Pose, error) { - return fk.sensorNoise, nil -} - -func (fk *fakePTGKinematics) ExecutionState(ctx context.Context) (motionplan.ExecutionState, error) { - fk.inputLock.RLock() - defer fk.inputLock.RUnlock() - pos, err := fk.CurrentPosition(ctx) - if err != nil { - return motionplan.ExecutionState{}, err - } - - return motionplan.NewExecutionState( - fk.plan, - fk.currentIndex, - map[string][]referenceframe.Input{fk.Kinematics().Name(): fk.currentInput}, - map[string]*referenceframe.PoseInFrame{fk.LocalizationFrame().Name(): pos}, - ) -} - -func (fk *fakePTGKinematics) CurrentPosition(ctx context.Context) (*referenceframe.PoseInFrame, error) { - return fk.localizer.CurrentPosition(ctx) -} - -type fakePTGKinematicsLocalizer struct { - fk *fakePTGKinematics -} - -func (fkl *fakePTGKinematicsLocalizer) CurrentPosition(ctx context.Context) (*referenceframe.PoseInFrame, error) { - fkl.fk.positionlock.RLock() - defer fkl.fk.positionlock.RUnlock() - origin := fkl.fk.origin - return referenceframe.NewPoseInFrame(origin.Parent(), spatialmath.Compose(origin.Pose(), fkl.fk.sensorNoise)), nil -} - -func inputsToPlan( - inputs [][]referenceframe.Input, - startPose *referenceframe.PoseInFrame, - frame referenceframe.Frame, -) (motionplan.Plan, error) { - runningPose := startPose.Pose() - traj := motionplan.Trajectory{} - path := motionplan.Path{} - for _, input := range inputs { - inputPose, err := frame.Transform(input) - if err != nil { - return nil, err - } - runningPose = spatialmath.Compose(runningPose, inputPose) - traj = append(traj, map[string][]referenceframe.Input{frame.Name(): input}) - path = append(path, map[string]*referenceframe.PoseInFrame{ - frame.Name(): referenceframe.NewPoseInFrame(startPose.Parent(), runningPose), - }) - } - - return motionplan.NewSimplePlan(path, traj), nil -} From 093469c1769e91fc4ae5e19777c3d02f93961271 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 3 Jul 2024 15:44:20 -0400 Subject: [PATCH 082/126] make TestObstacleReplanningGlobe/ensure_replan_due_to_obstacle_collision NOT FLAKY --- motionplan/check.go | 2 ++ services/motion/builtin/move_on_globe_test.go | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/motionplan/check.go b/motionplan/check.go index eca4e701741..a544a233c8f 100644 --- a/motionplan/check.go +++ b/motionplan/check.go @@ -155,6 +155,8 @@ func checkPlanRelative( ); err != nil { return err } + // change from 60mm to 30mm so we have finer interpolation along segments + sfPlanner.planOpts.Resolution = 30 currentInputs := executionState.CurrentInputs() wayPointIdx := executionState.Index() diff --git a/services/motion/builtin/move_on_globe_test.go b/services/motion/builtin/move_on_globe_test.go index 57a93e6b2c0..41e43b62d6d 100644 --- a/services/motion/builtin/move_on_globe_test.go +++ b/services/motion/builtin/move_on_globe_test.go @@ -314,7 +314,7 @@ func TestObstacleReplanningGlobe(t *testing.T) { // 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) + box, err := spatialmath.NewBox(obstaclePosition, r3.Vector{X: 40, Y: 40, Z: 10}, caseName) test.That(t, err, test.ShouldBeNil) detection, err := viz.NewObjectWithLabel(pointcloud.New(), caseName+"-detection", box.ToProtobuf()) @@ -362,7 +362,7 @@ func TestObstacleReplanningGlobe(t *testing.T) { ctx, t, gpsOrigin, - 1, + 2, nil, ) defer closeFunc(ctx) From f43694ded8023ad831267c3bc2e5ec3fbce53aa2 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 10 Jul 2024 15:12:40 -0400 Subject: [PATCH 083/126] changes requested from Peter part1 --- components/base/kinematicbase/ptgKinematics.go | 5 +---- motionplan/check.go | 17 +++++++++-------- referenceframe/frame.go | 12 ++++++++---- services/motion/builtin/move_request.go | 5 ++++- services/motion/builtin/wrapperFrame.go | 15 +++++++++++++-- 5 files changed, 35 insertions(+), 19 deletions(-) diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index 74976d38a37..5242f29e480 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -141,10 +141,7 @@ func wrapWithPTGKinematics( } startingState := baseState{currentInputs: zeroInput} - // we intentionally set our OX, OY, OZ values to be greater than 1 since - // a possible orientation which a planner produces may have the following - // values: OZ: 1.0000000002, Theta: t. - localizationFrame, err := referenceframe.NewPoseFrame(b.Name().ShortName()+"LocalizationFrame", nil) + localizationFrame, err := referenceframe.NewPoseFrame(b.Name().ShortName()+"_LocalizationFrame", nil) if err != nil { return nil, err } diff --git a/motionplan/check.go b/motionplan/check.go index a544a233c8f..54aee7e94e8 100644 --- a/motionplan/check.go +++ b/motionplan/check.go @@ -55,9 +55,9 @@ func CheckPlan( 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 DoF of checkFrame is 11, indication that is a wrapper frame, - // housing the ptgFrame with 4 Dof and localization frame with 7 DoF. - if len(checkFrame.DoF()) == 11 { + // Currently this is only TP-space, so we check if the PTG length is >0. + // The solver frame will have had its PTGs filled in the newPlanManager() call, if applicable. + if sfPlanner.useTPspace { return checkPlanRelative(checkFrame, executionState, worldState, fs, lookAheadDistanceMM, sfPlanner) } return checkPlanAbsolute(checkFrame, executionState, worldState, fs, lookAheadDistanceMM, sfPlanner) @@ -126,7 +126,7 @@ func checkPlanRelative( // determine plan's starting pose planStartPiF, ok := plan.Path()[0][checkFrame.Name()] if !ok { - return errCheckFrameNotInPath // THIS ERROR APPEARS TWICE, SO WE CAN MAKE IT INTO A VAR + return errCheckFrameNotInPath } planStartPoseWorld, err := toWorld(planStartPiF, plan.Trajectory()[0]) if err != nil { @@ -162,7 +162,7 @@ func checkPlanRelative( wayPointIdx := executionState.Index() sf := sfPlanner.frame - // construct first segmenet's start configuration + // 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) @@ -170,14 +170,14 @@ func checkPlanRelative( return err } - // construct first segmenet's end configuration + // construct first segment's end configuration currentWayPointTraj := plan.Trajectory()[wayPointIdx] arcEndInputs, err := sf.mapToSlice(currentWayPointTraj) if err != nil { return err } - // construct first segmenet's startPosition + // construct first segment's startPosition currentPoses := executionState.CurrentPoses() if currentPoses == nil { return errors.New("executionState had nil return from CurrentPoses") @@ -192,7 +192,7 @@ func checkPlanRelative( return err } - // construct first segmenet's endPosition + // construct first segment's endPosition // Check that path pose is valid stepEndPiF, ok := plan.Path()[wayPointIdx][checkFrame.Name()] if !ok { @@ -230,6 +230,7 @@ func checkPlanRelative( 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(), EndPosition: currentArcEndPose, diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 558773d0ac7..33c014a0c1d 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -448,12 +448,12 @@ func (rf rotationalFrame) MarshalJSON() ([]byte, error) { type poseFrame struct { *baseFrame - geometry spatial.Geometry + 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) { +func NewPoseFrame(name string, geometry []spatial.Geometry) (Frame, error) { orientationVector := spatial.OrientationVector{ OX: math.Inf(1), OY: math.Inf(1), @@ -511,10 +511,14 @@ func (pf *poseFrame) Geometries(inputs []Input) (*GeometriesInFrame, error) { if err != nil { return nil, err } - if pf.geometry == nil { + if len(pf.geometries) == 0 { return NewGeometriesInFrame(pf.name, []spatial.Geometry{}), nil } - return NewGeometriesInFrame(pf.name, []spatial.Geometry{pf.geometry.Transform(transformByPose)}), 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. diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 0853db38005..f702f44710e 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -754,7 +754,10 @@ func (ms *builtIn) createBaseMoveRequest( wrapperFrameSeedMap := map[string][]referenceframe.Input{} wrapperFrameSeedMap[executionFrame.Name()] = make([]referenceframe.Input, len(executionFrame.DoF())) wrapperFrameSeedMap[localizationFrame.Name()] = make([]referenceframe.Input, len(localizationFrame.DoF())) - wf := newWrapperFrame(localizationFrame, executionFrame, wrapperFrameSeedMap, wrapperFS) + wf, err := newWrapperFrame(localizationFrame, executionFrame, wrapperFrameSeedMap, wrapperFS) + if err != nil { + return nil, err + } collisionFS := baseOnlyFS err = collisionFS.ReplaceFrame(wf) if err != nil { diff --git a/services/motion/builtin/wrapperFrame.go b/services/motion/builtin/wrapperFrame.go index e6864ff8897..0ec6bc6247f 100644 --- a/services/motion/builtin/wrapperFrame.go +++ b/services/motion/builtin/wrapperFrame.go @@ -5,6 +5,7 @@ import ( "go.uber.org/multierr" 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" @@ -18,20 +19,26 @@ type wrapperFrame struct { executionFrame referenceframe.Frame seedMap map[string][]referenceframe.Input fs referenceframe.FrameSystem + ptgSolvers []tpspace.PTGSolver } func newWrapperFrame( localizationFrame, executionFrame referenceframe.Frame, seedMap map[string][]referenceframe.Input, fs referenceframe.FrameSystem, -) referenceframe.Frame { +) (referenceframe.Frame, error) { + ptgFrame, ok := executionFrame.(tpspace.PTGProvider) + if !ok { + return nil, errors.New("cannot type assert localizationFrame into a tpspace.PTGProvider") + } return &wrapperFrame{ name: executionFrame.Name(), localizationFrame: localizationFrame, executionFrame: executionFrame, seedMap: seedMap, fs: fs, - } + ptgSolvers: ptgFrame.PTGSolvers(), + }, nil } // Name returns the name of the wrapper frame's execution frame's name. @@ -157,3 +164,7 @@ func (wf *wrapperFrame) sliceToMap(inputSlice []referenceframe.Input) map[string inputs[wf.localizationFrame.Name()] = inputSlice[len(wf.executionFrame.DoF()):] return inputs } + +func (wf *wrapperFrame) PTGSolvers() []tpspace.PTGSolver { + return wf.ptgSolvers +} From 4c4f85724d715d7c0d5b9b0aef2842b0a3d93e4d Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 10 Jul 2024 17:29:07 -0400 Subject: [PATCH 084/126] changes requested from Peter part2 --- motionplan/check.go | 39 ++----- services/motion/builtin/builtin_test.go | 33 +++--- services/motion/builtin/move_on_map_test.go | 4 +- services/motion/builtin/move_request.go | 110 ++++++++++++++------ services/sensors/client.go | 16 +-- services/sensors/server.go | 24 ++--- services/sensors/server_test.go | 28 ++--- 7 files changed, 142 insertions(+), 112 deletions(-) diff --git a/motionplan/check.go b/motionplan/check.go index 54aee7e94e8..af7e3c417bf 100644 --- a/motionplan/check.go +++ b/motionplan/check.go @@ -104,11 +104,6 @@ func checkPlanRelative( } 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 @@ -122,23 +117,16 @@ func checkPlanRelative( } plan := executionState.Plan() + zeroPosePIF := referenceframe.NewPoseInFrame(checkFrame.Name(), spatialmath.NewZeroPose()) // determine plan's starting pose - planStartPiF, ok := plan.Path()[0][checkFrame.Name()] - if !ok { - return errCheckFrameNotInPath - } - planStartPoseWorld, err := toWorld(planStartPiF, plan.Trajectory()[0]) + planStartPoseWorld, err := toWorld(zeroPosePIF, plan.Trajectory()[0]) if err != nil { return err } // determine plan's ending pose - planEndPiF, ok := plan.Path()[len(plan.Path())-1][checkFrame.Name()] - if !ok { - return errCheckFrameNotInPath - } - planEndPoseWorld, err := toWorld(planEndPiF, plan.Trajectory()[len(plan.Path())-1]) + planEndPoseWorld, err := toWorld(zeroPosePIF, plan.Trajectory()[len(plan.Path())-1]) if err != nil { return err } @@ -171,8 +159,7 @@ func checkPlanRelative( } // construct first segment's end configuration - currentWayPointTraj := plan.Trajectory()[wayPointIdx] - arcEndInputs, err := sf.mapToSlice(currentWayPointTraj) + arcEndInputs, err := sf.mapToSlice(plan.Trajectory()[wayPointIdx]) if err != nil { return err } @@ -187,19 +174,17 @@ func checkPlanRelative( if !ok { return errors.New("checkFrame not found in current pose map") } + err = validateRelPiF(currentPoseIF) + if err != nil { + return err + } currentPoseInWorld, err := toWorld(currentPoseIF, currentInputs) if err != nil { return err } // construct first segment's endPosition - // Check that path pose is valid - stepEndPiF, ok := plan.Path()[wayPointIdx][checkFrame.Name()] - if !ok { - return errCheckFrameNotInPath - } - - expectedArcEndInWorld, err := toWorld(stepEndPiF, plan.Trajectory()[wayPointIdx]) + expectedArcEndInWorld, err := toWorld(zeroPosePIF, plan.Trajectory()[wayPointIdx]) if err != nil { return err } @@ -243,11 +228,7 @@ func checkPlanRelative( // 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 errCheckFrameNotInPath - } - thisArcEndPoseInWorld, err := toWorld(thisArcEndPoseTf, plan.Trajectory()[i]) + thisArcEndPoseInWorld, err := toWorld(zeroPosePIF, plan.Trajectory()[i]) if err != nil { return err } diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 603b14c14ab..e4c25e389e7 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -2,7 +2,6 @@ package builtin import ( "context" - "fmt" "math" "strings" "testing" @@ -289,13 +288,13 @@ func TestPositionalReplanning(t *testing.T) { // expectedSuccess: false, // extra: map[string]interface{}{"replan_cost_factor": 0.01, "smooth_iter": 5}, // }, - { - name: "check we replan with a noisy sensor", - noise: r3.Vector{Y: epsilonMM + 0.1}, - expectedErr: fmt.Sprintf("exceeded maximum number of replans: %d: plan failed", 4), - expectedSuccess: false, - extra: map[string]interface{}{"replan_cost_factor": 10.0, "max_replans": 4, "smooth_iter": 5}, - }, + // { + // name: "check we replan with a noisy sensor", + // noise: r3.Vector{Y: epsilonMM + 0.1}, + // expectedErr: fmt.Sprintf("exceeded maximum number of replans: %d: plan failed", 4), + // expectedSuccess: false, + // extra: map[string]interface{}{"replan_cost_factor": 10.0, "max_replans": 4, "smooth_iter": 5}, + // }, } testFn := func(t *testing.T, tc testCase) { @@ -946,7 +945,7 @@ func TestCheckPlan(t *testing.T) { test.That(t, err, test.ShouldBeNil) test.That(t, len(plan.Path()), test.ShouldBeGreaterThan, 2) - wrapperFrame := mr.localizaingFS.Frame(mr.kinematicBase.Name().Name) + wrapperFrame := mr.localizingFS.Frame(mr.kinematicBase.Name().Name) currentInputs := map[string][]referenceframe.Input{ mr.kinematicBase.Kinematics().Name(): { @@ -981,7 +980,7 @@ func TestCheckPlan(t *testing.T) { 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.localizaingFS, math.Inf(1), logger) + err = motionplan.CheckPlan(wrapperFrame, augmentedBaseExecutionState, nil, mr.localizingFS, math.Inf(1), logger) test.That(t, err, test.ShouldBeNil) }) @@ -995,7 +994,7 @@ func TestCheckPlan(t *testing.T) { worldState, err := referenceframe.NewWorldState(gifs, nil) test.That(t, err, test.ShouldBeNil) - err = motionplan.CheckPlan(wrapperFrame, augmentedBaseExecutionState, worldState, mr.localizaingFS, math.Inf(1), logger) + 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) }) @@ -1003,7 +1002,7 @@ func TestCheckPlan(t *testing.T) { // 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.localizaingFS.AddFrame(cameraOriginFrame, wrapperFrame) + err = mr.localizingFS.AddFrame(cameraOriginFrame, wrapperFrame) test.That(t, err, test.ShouldBeNil) // create camera geometry @@ -1015,7 +1014,7 @@ func TestCheckPlan(t *testing.T) { "camera-frame", spatialmath.NewPoseFromPoint(r3.Vector{0, 0, 0}), cameraGeom, ) test.That(t, err, test.ShouldBeNil) - err = mr.localizaingFS.AddFrame(cameraFrame, cameraOriginFrame) + 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()))) @@ -1037,7 +1036,7 @@ func TestCheckPlan(t *testing.T) { worldState, err := referenceframe.NewWorldState(gifs, nil) test.That(t, err, test.ShouldBeNil) - err = motionplan.CheckPlan(wrapperFrame, executionStateWithCamera, worldState, mr.localizaingFS, math.Inf(1), logger) + err = motionplan.CheckPlan(wrapperFrame, executionStateWithCamera, worldState, mr.localizingFS, math.Inf(1), logger) test.That(t, err, test.ShouldBeNil) }) @@ -1053,7 +1052,7 @@ func TestCheckPlan(t *testing.T) { worldState, err := referenceframe.NewWorldState(gifs, nil) test.That(t, err, test.ShouldBeNil) - err = motionplan.CheckPlan(wrapperFrame, executionStateWithCamera, worldState, mr.localizaingFS, math.Inf(1), logger) + 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) }) @@ -1102,7 +1101,7 @@ func TestCheckPlan(t *testing.T) { worldState, err := referenceframe.NewWorldState(gifs, nil) test.That(t, err, test.ShouldBeNil) - err = motionplan.CheckPlan(wrapperFrame, updatedExecutionState, worldState, mr.localizaingFS, math.Inf(1), logger) + err = motionplan.CheckPlan(wrapperFrame, updatedExecutionState, worldState, mr.localizingFS, math.Inf(1), logger) test.That(t, err, test.ShouldBeNil) }) @@ -1115,7 +1114,7 @@ func TestCheckPlan(t *testing.T) { worldState, err := referenceframe.NewWorldState(gifs, nil) test.That(t, err, test.ShouldBeNil) - err = motionplan.CheckPlan(wrapperFrame, updatedExecutionState, worldState, mr.localizaingFS, math.Inf(1), logger) + 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_map_test.go b/services/motion/builtin/move_on_map_test.go index e24c792d67e..97a53252f9b 100644 --- a/services/motion/builtin/move_on_map_test.go +++ b/services/motion/builtin/move_on_map_test.go @@ -308,14 +308,14 @@ func TestMoveOnMapStaticObs(t *testing.T) { augmentedBaseExecutionState, err := mr.augmentBaseExecutionState(baseExecutionState) test.That(t, err, test.ShouldBeNil) - wrapperFrame := mr.localizaingFS.Frame(mr.kinematicBase.Name().Name) + wrapperFrame := mr.localizingFS.Frame(mr.kinematicBase.Name().Name) test.That(t, err, test.ShouldBeNil) err = motionplan.CheckPlan( wrapperFrame, augmentedBaseExecutionState, wrldSt, - mr.localizaingFS, + mr.localizingFS, lookAheadDistanceMM, logger, ) diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index f702f44710e..b3390cc3478 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" @@ -66,7 +67,13 @@ type moveRequest struct { obstacleDetectors map[vision.Service][]resource.Name replanCostFactor float64 fsService framesystem.Service - localizaingFS referenceframe.FrameSystem + // 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 @@ -209,7 +216,7 @@ func (mr *moveRequest) getTransientDetections( label += "_" + geometry.Label() } geometry.SetLabel(label) - tf, err := mr.localizaingFS.Transform( + tf, err := mr.localizingFS.Transform( inputMap, referenceframe.NewGeometriesInFrame(camName.ShortName(), []spatialmath.Geometry{geometry}), referenceframe.World, @@ -274,9 +281,13 @@ 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 - updatedBaseExecutionState, err := mr.augmentBaseExecutionState(baseExecutionState) - 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", @@ -284,12 +295,11 @@ func (mr *moveRequest) obstaclesIntersectPlan( updatedBaseExecutionState.CurrentInputs(), worldState.String(), ) - if err := motionplan.CheckPlan( - mr.localizaingFS.Frame(mr.kinematicBase.Kinematics().Name()), // frame we wish to check for collisions + 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.localizaingFS, + mr.localizingFS, lookAheadDistanceMM, mr.planRequest.Logger, ); err != nil { @@ -301,6 +311,15 @@ 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 baseExectionState. +// 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 +// kinematicBase.Kinematics. func (mr *moveRequest) augmentBaseExecutionState( baseExecutionState motionplan.ExecutionState, ) (motionplan.ExecutionState, error) { @@ -547,13 +566,9 @@ func (ms *builtIn) newMoveOnGlobeRequest( // these limits need to be updated so that they are in 7DOF // what is a good limit for what OX, OY, OZ should be? limits := []referenceframe.Limit{ - {Min: -straightlineDistance * 3, Max: straightlineDistance * 3}, // X - {Min: -straightlineDistance * 3, Max: straightlineDistance * 3}, // Y - {Min: -straightlineDistance * 3, Max: straightlineDistance * 3}, // Z - {Min: -1, Max: 1}, // OX - {Min: -1, Max: 1}, // OY - {Min: -1, Max: 1}, // OZ - {Min: -2 * math.Pi, Max: 2 * math.Pi}, // Theta + {Min: -straightlineDistance * 3, Max: straightlineDistance * 3}, + {Min: -straightlineDistance * 3, Max: straightlineDistance * 3}, + {Min: -2 * math.Pi, Max: 2 * math.Pi}, } // Note: this is only for diff drive, not used for PTGs kb, err := kinematicbase.WrapWithKinematics(ctx, b, ms.logger, localizer, limits, kinematicsOptions) if err != nil { @@ -634,6 +649,7 @@ func (ms *builtIn) newMoveOnMapRequest( if err != nil { return nil, err } + limits = append(limits, referenceframe.Limit{Min: -2 * math.Pi, Max: 2 * math.Pi}) // create a KinematicBase from the componentName component, ok := ms.components[req.ComponentName] @@ -724,22 +740,56 @@ func (ms *builtIn) createBaseMoveRequest( return nil, err } - // create fs used for planning - 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 + 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 + } } - // create wrapperFrame and fs used for collision checks + // // 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 and a collision framesystem 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() wrapperFS := referenceframe.NewEmptyFrameSystem("wrapperFS") @@ -840,7 +890,7 @@ func (ms *builtIn) createBaseMoveRequest( replanCostFactor: valExtra.replanCostFactor, obstacleDetectors: obstacleDetectors, fsService: ms.fsService, - localizaingFS: collisionFS, + localizingFS: collisionFS, executeBackgroundWorkers: &backgroundWorkers, diff --git a/services/sensors/client.go b/services/sensors/client.go index 9adbfdfe3fc..72c1555d770 100644 --- a/services/sensors/client.go +++ b/services/sensors/client.go @@ -47,14 +47,14 @@ func (c *client) Sensors(ctx context.Context, extra map[string]interface{}) ([]r if err != nil { return nil, err } - //nolint:staticcheck + resp, err := c.client.GetSensors(ctx, &pb.GetSensorsRequest{Name: c.name, Extra: ext}) if err != nil { return nil, err } - //nolint:staticcheck + sensorNames := make([]resource.Name, 0, len(resp.SensorNames)) - //nolint:staticcheck + for _, name := range resp.SensorNames { sensorNames = append(sensorNames, rprotoutils.ResourceNameFromProto(name)) } @@ -70,24 +70,24 @@ func (c *client) Readings(ctx context.Context, sensorNames []resource.Name, extr if err != nil { return nil, err } - //nolint:staticcheck + resp, err := c.client.GetReadings(ctx, &pb.GetReadingsRequest{Name: c.name, SensorNames: names, Extra: ext}) if err != nil { return nil, err } - //nolint:staticcheck + readings := make([]Readings, 0, len(resp.Readings)) - //nolint:staticcheck + for _, reading := range resp.Readings { - //nolint:staticcheck + sReading, err := rprotoutils.ReadingProtoToGo(reading.Readings) if err != nil { return nil, err } readings = append( readings, Readings{ - //nolint:staticcheck + Name: rprotoutils.ResourceNameFromProto(reading.Name), Readings: sReading, }) diff --git a/services/sensors/server.go b/services/sensors/server.go index 801e97009a8..74efc7289ad 100644 --- a/services/sensors/server.go +++ b/services/sensors/server.go @@ -23,17 +23,17 @@ func NewRPCServiceServer(coll resource.APIResourceCollection[Service]) interface return &serviceServer{coll: coll} } -//nolint:staticcheck + func (server *serviceServer) GetSensors( ctx context.Context, req *pb.GetSensorsRequest, ) (*pb.GetSensorsResponse, error) { - //nolint:staticcheck + svc, err := server.coll.Resource(req.Name) if err != nil { return nil, err } - //nolint:staticcheck + names, err := svc.Sensors(ctx, req.Extra.AsMap()) if err != nil { return nil, err @@ -43,41 +43,41 @@ func (server *serviceServer) GetSensors( sensorNames = append(sensorNames, protoutils.ResourceNameToProto(name)) } - //nolint:staticcheck + return &pb.GetSensorsResponse{SensorNames: sensorNames}, nil } -//nolint:staticcheck + func (server *serviceServer) GetReadings( ctx context.Context, req *pb.GetReadingsRequest, ) (*pb.GetReadingsResponse, error) { - //nolint:staticcheck + svc, err := server.coll.Resource(req.Name) if err != nil { return nil, err } - //nolint:staticcheck + sensorNames := make([]resource.Name, 0, len(req.SensorNames)) - //nolint:staticcheck + for _, name := range req.SensorNames { sensorNames = append(sensorNames, protoutils.ResourceNameFromProto(name)) } - //nolint:staticcheck + readings, err := svc.Readings(ctx, sensorNames, req.Extra.AsMap()) if err != nil { return nil, err } - //nolint:staticcheck + readingsP := make([]*pb.Readings, 0, len(readings)) for _, reading := range readings { rReading, err := protoutils.ReadingGoToProto(reading.Readings) if err != nil { return nil, err } - //nolint:staticcheck + readingP := &pb.Readings{ Name: protoutils.ResourceNameToProto(reading.Name), Readings: rReading, @@ -85,7 +85,7 @@ func (server *serviceServer) GetReadings( readingsP = append(readingsP, readingP) } - //nolint:staticcheck + return &pb.GetReadingsResponse{Readings: readingsP}, nil } diff --git a/services/sensors/server_test.go b/services/sensors/server_test.go index 55744f23577..aac86d1cd26 100644 --- a/services/sensors/server_test.go +++ b/services/sensors/server_test.go @@ -32,7 +32,7 @@ func TestServerGetSensors(t *testing.T) { sMap := map[resource.Name]sensors.Service{} server, err := newServer(sMap) test.That(t, err, test.ShouldBeNil) - //nolint:staticcheck + _, err = server.GetSensors(context.Background(), &pb.GetSensorsRequest{}) test.That(t, err, test.ShouldBeError, errors.New("resource \"rdk:service:sensors/\" not found")) }) @@ -49,7 +49,7 @@ func TestServerGetSensors(t *testing.T) { return nil, passedErr } - //nolint:staticcheck + _, err = server.GetSensors(context.Background(), &pb.GetSensorsRequest{Name: testSvcName1.ShortName()}) test.That(t, err, test.ShouldBeError, passedErr) }) @@ -72,15 +72,15 @@ func TestServerGetSensors(t *testing.T) { ext, err := protoutils.StructToStructPb(extra) test.That(t, err, test.ShouldBeNil) - //nolint:staticcheck + resp, err := server.GetSensors(context.Background(), &pb.GetSensorsRequest{Name: testSvcName1.ShortName(), Extra: ext}) test.That(t, err, test.ShouldBeNil) test.That(t, extraOptions, test.ShouldResemble, extra) - //nolint:staticcheck + convertedNames := make([]resource.Name, 0, len(resp.SensorNames)) - //nolint:staticcheck + for _, rn := range resp.SensorNames { convertedNames = append(convertedNames, rprotoutils.ResourceNameFromProto(rn)) } @@ -94,7 +94,7 @@ func TestServerGetReadings(t *testing.T) { server, err := newServer(sMap) test.That(t, err, test.ShouldBeNil) - //nolint:staticcheck + _, err = server.GetReadings(context.Background(), &pb.GetReadingsRequest{}) test.That(t, err, test.ShouldBeError, errors.New("resource \"rdk:service:sensors/\" not found")) }) @@ -113,13 +113,13 @@ func TestServerGetReadings(t *testing.T) { return nil, passedErr } - //nolint:staticcheck + req := &pb.GetReadingsRequest{ Name: testSvcName1.ShortName(), SensorNames: []*commonpb.ResourceName{}, } - //nolint:staticcheck + _, err = server.GetReadings(context.Background(), req) test.That(t, err, test.ShouldBeError, passedErr) }) @@ -149,18 +149,18 @@ func TestServerGetReadings(t *testing.T) { ext, err := protoutils.StructToStructPb(extra) test.That(t, err, test.ShouldBeNil) - //nolint:staticcheck + req := &pb.GetReadingsRequest{ Name: testSvcName1.ShortName(), SensorNames: []*commonpb.ResourceName{}, Extra: ext, } - //nolint:staticcheck + resp, err := server.GetReadings(context.Background(), req) test.That(t, err, test.ShouldBeNil) - //nolint:staticcheck + test.That(t, len(resp.Readings), test.ShouldEqual, 2) test.That(t, extraOptions, test.ShouldResemble, extra) @@ -173,10 +173,10 @@ func TestServerGetReadings(t *testing.T) { } observed := map[resource.Name]interface{}{ - //nolint:staticcheck + rprotoutils.ResourceNameFromProto(resp.Readings[0].Name): conv(resp.Readings[0].Readings), - //nolint:staticcheck + rprotoutils.ResourceNameFromProto(resp.Readings[1].Name): conv(resp.Readings[1].Readings), } test.That(t, observed, test.ShouldResemble, expected) @@ -199,7 +199,7 @@ func TestServerDoCommand(t *testing.T) { Command: cmd, } - //nolint:staticcheck + doCommandResponse, err := server.DoCommand(context.Background(), doCommandRequest) test.That(t, err, test.ShouldBeNil) From 60873d319f2a372730b4d3b5412131ab7f7a1ee1 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 10 Jul 2024 17:30:34 -0400 Subject: [PATCH 085/126] remove old commented out code --- services/motion/builtin/move_request.go | 21 --------------------- 1 file changed, 21 deletions(-) diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 14660ba7342..77292b1007b 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -772,27 +772,6 @@ func (ms *builtIn) createBaseMoveRequest( } } - // // 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 and a collision framesystem 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 From 2f0d74418af7cac56cd1f7aa1b4fe7b0c7cbe3b7 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 10 Jul 2024 17:49:55 -0400 Subject: [PATCH 086/126] revert sensor code --- services/sensors/client.go | 16 ++++++++-------- services/sensors/server.go | 24 ++++++++++++------------ services/sensors/server_test.go | 28 ++++++++++++++-------------- 3 files changed, 34 insertions(+), 34 deletions(-) diff --git a/services/sensors/client.go b/services/sensors/client.go index 72c1555d770..9adbfdfe3fc 100644 --- a/services/sensors/client.go +++ b/services/sensors/client.go @@ -47,14 +47,14 @@ func (c *client) Sensors(ctx context.Context, extra map[string]interface{}) ([]r if err != nil { return nil, err } - + //nolint:staticcheck resp, err := c.client.GetSensors(ctx, &pb.GetSensorsRequest{Name: c.name, Extra: ext}) if err != nil { return nil, err } - + //nolint:staticcheck sensorNames := make([]resource.Name, 0, len(resp.SensorNames)) - + //nolint:staticcheck for _, name := range resp.SensorNames { sensorNames = append(sensorNames, rprotoutils.ResourceNameFromProto(name)) } @@ -70,24 +70,24 @@ func (c *client) Readings(ctx context.Context, sensorNames []resource.Name, extr if err != nil { return nil, err } - + //nolint:staticcheck resp, err := c.client.GetReadings(ctx, &pb.GetReadingsRequest{Name: c.name, SensorNames: names, Extra: ext}) if err != nil { return nil, err } - + //nolint:staticcheck readings := make([]Readings, 0, len(resp.Readings)) - + //nolint:staticcheck for _, reading := range resp.Readings { - + //nolint:staticcheck sReading, err := rprotoutils.ReadingProtoToGo(reading.Readings) if err != nil { return nil, err } readings = append( readings, Readings{ - + //nolint:staticcheck Name: rprotoutils.ResourceNameFromProto(reading.Name), Readings: sReading, }) diff --git a/services/sensors/server.go b/services/sensors/server.go index 74efc7289ad..801e97009a8 100644 --- a/services/sensors/server.go +++ b/services/sensors/server.go @@ -23,17 +23,17 @@ func NewRPCServiceServer(coll resource.APIResourceCollection[Service]) interface return &serviceServer{coll: coll} } - +//nolint:staticcheck func (server *serviceServer) GetSensors( ctx context.Context, req *pb.GetSensorsRequest, ) (*pb.GetSensorsResponse, error) { - + //nolint:staticcheck svc, err := server.coll.Resource(req.Name) if err != nil { return nil, err } - + //nolint:staticcheck names, err := svc.Sensors(ctx, req.Extra.AsMap()) if err != nil { return nil, err @@ -43,41 +43,41 @@ func (server *serviceServer) GetSensors( sensorNames = append(sensorNames, protoutils.ResourceNameToProto(name)) } - + //nolint:staticcheck return &pb.GetSensorsResponse{SensorNames: sensorNames}, nil } - +//nolint:staticcheck func (server *serviceServer) GetReadings( ctx context.Context, req *pb.GetReadingsRequest, ) (*pb.GetReadingsResponse, error) { - + //nolint:staticcheck svc, err := server.coll.Resource(req.Name) if err != nil { return nil, err } - + //nolint:staticcheck sensorNames := make([]resource.Name, 0, len(req.SensorNames)) - + //nolint:staticcheck for _, name := range req.SensorNames { sensorNames = append(sensorNames, protoutils.ResourceNameFromProto(name)) } - + //nolint:staticcheck readings, err := svc.Readings(ctx, sensorNames, req.Extra.AsMap()) if err != nil { return nil, err } - + //nolint:staticcheck readingsP := make([]*pb.Readings, 0, len(readings)) for _, reading := range readings { rReading, err := protoutils.ReadingGoToProto(reading.Readings) if err != nil { return nil, err } - + //nolint:staticcheck readingP := &pb.Readings{ Name: protoutils.ResourceNameToProto(reading.Name), Readings: rReading, @@ -85,7 +85,7 @@ func (server *serviceServer) GetReadings( readingsP = append(readingsP, readingP) } - + //nolint:staticcheck return &pb.GetReadingsResponse{Readings: readingsP}, nil } diff --git a/services/sensors/server_test.go b/services/sensors/server_test.go index aac86d1cd26..55744f23577 100644 --- a/services/sensors/server_test.go +++ b/services/sensors/server_test.go @@ -32,7 +32,7 @@ func TestServerGetSensors(t *testing.T) { sMap := map[resource.Name]sensors.Service{} server, err := newServer(sMap) test.That(t, err, test.ShouldBeNil) - + //nolint:staticcheck _, err = server.GetSensors(context.Background(), &pb.GetSensorsRequest{}) test.That(t, err, test.ShouldBeError, errors.New("resource \"rdk:service:sensors/\" not found")) }) @@ -49,7 +49,7 @@ func TestServerGetSensors(t *testing.T) { return nil, passedErr } - + //nolint:staticcheck _, err = server.GetSensors(context.Background(), &pb.GetSensorsRequest{Name: testSvcName1.ShortName()}) test.That(t, err, test.ShouldBeError, passedErr) }) @@ -72,15 +72,15 @@ func TestServerGetSensors(t *testing.T) { ext, err := protoutils.StructToStructPb(extra) test.That(t, err, test.ShouldBeNil) - + //nolint:staticcheck resp, err := server.GetSensors(context.Background(), &pb.GetSensorsRequest{Name: testSvcName1.ShortName(), Extra: ext}) test.That(t, err, test.ShouldBeNil) test.That(t, extraOptions, test.ShouldResemble, extra) - + //nolint:staticcheck convertedNames := make([]resource.Name, 0, len(resp.SensorNames)) - + //nolint:staticcheck for _, rn := range resp.SensorNames { convertedNames = append(convertedNames, rprotoutils.ResourceNameFromProto(rn)) } @@ -94,7 +94,7 @@ func TestServerGetReadings(t *testing.T) { server, err := newServer(sMap) test.That(t, err, test.ShouldBeNil) - + //nolint:staticcheck _, err = server.GetReadings(context.Background(), &pb.GetReadingsRequest{}) test.That(t, err, test.ShouldBeError, errors.New("resource \"rdk:service:sensors/\" not found")) }) @@ -113,13 +113,13 @@ func TestServerGetReadings(t *testing.T) { return nil, passedErr } - + //nolint:staticcheck req := &pb.GetReadingsRequest{ Name: testSvcName1.ShortName(), SensorNames: []*commonpb.ResourceName{}, } - + //nolint:staticcheck _, err = server.GetReadings(context.Background(), req) test.That(t, err, test.ShouldBeError, passedErr) }) @@ -149,18 +149,18 @@ func TestServerGetReadings(t *testing.T) { ext, err := protoutils.StructToStructPb(extra) test.That(t, err, test.ShouldBeNil) - + //nolint:staticcheck req := &pb.GetReadingsRequest{ Name: testSvcName1.ShortName(), SensorNames: []*commonpb.ResourceName{}, Extra: ext, } - + //nolint:staticcheck resp, err := server.GetReadings(context.Background(), req) test.That(t, err, test.ShouldBeNil) - + //nolint:staticcheck test.That(t, len(resp.Readings), test.ShouldEqual, 2) test.That(t, extraOptions, test.ShouldResemble, extra) @@ -173,10 +173,10 @@ func TestServerGetReadings(t *testing.T) { } observed := map[resource.Name]interface{}{ - + //nolint:staticcheck rprotoutils.ResourceNameFromProto(resp.Readings[0].Name): conv(resp.Readings[0].Readings), - + //nolint:staticcheck rprotoutils.ResourceNameFromProto(resp.Readings[1].Name): conv(resp.Readings[1].Readings), } test.That(t, observed, test.ShouldResemble, expected) @@ -199,7 +199,7 @@ func TestServerDoCommand(t *testing.T) { Command: cmd, } - + //nolint:staticcheck doCommandResponse, err := server.DoCommand(context.Background(), doCommandRequest) test.That(t, err, test.ShouldBeNil) From 573e123e2556ea1b0dfdf9021cbe14b3456b5e69 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 10 Jul 2024 18:07:12 -0400 Subject: [PATCH 087/126] fix CalculateFrameErrorState --- .../base/kinematicbase/ptgKinematics_test.go | 4 ++-- motionplan/plan.go | 16 ++++++++-------- services/motion/builtin/move_request.go | 2 +- 3 files changed, 11 insertions(+), 11 deletions(-) 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/plan.go b/motionplan/plan.go index 7832f3842cd..2600e0f9fd0 100644 --- a/motionplan/plan.go +++ b/motionplan/plan.go @@ -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, fmt.Errorf("1could not find frame %s in ExecutionState", 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, fmt.Errorf("2could not find frame %s in ExecutionState", 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, fmt.Errorf("could not find frame %s in ExecutionState", executionFrame.Name()) } if pose.Parent() != currentPose.Parent() { return nil, errors.New("cannot compose two PoseInFrames with different parents") diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 77292b1007b..2b45fa2a626 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -166,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 } From ebb3b581ed38bd03571bbc4bcff5019dbd7398b4 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 11 Jul 2024 10:30:03 -0400 Subject: [PATCH 088/126] add comments to wrapperFrame interpolate function and update variable names --- services/motion/builtin/wrapperFrame.go | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/services/motion/builtin/wrapperFrame.go b/services/motion/builtin/wrapperFrame.go index 0ec6bc6247f..0331ef1a1f5 100644 --- a/services/motion/builtin/wrapperFrame.go +++ b/services/motion/builtin/wrapperFrame.go @@ -69,19 +69,26 @@ func (wf *wrapperFrame) Interpolate(from, to []referenceframe.Input, by float64) } 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 + // executionFrame interpolation - fromSubset := from[:len(wf.executionFrame.DoF())] - toSubset := to[:len(wf.executionFrame.DoF())] - interpSub, err := wf.executionFrame.Interpolate(fromSubset, toSubset, by) + executionFrameFromSubset := from[: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...) // localizationFrame interpolation - fromSubset = from[len(wf.executionFrame.DoF()):] - toSubset = to[len(wf.executionFrame.DoF()):] - interpSub, err = wf.localizationFrame.Interpolate(fromSubset, toSubset, by) + // interpolating the localizationFrame is a special case + // the ToSubset of the localizationFrame does not matter since the executionFrame interpolations + // move us through a given segment and the localizationFrame input values are what we compose with + // the position ourselves in our absolute position in world. + localizationFrameFromSubset := from[len(wf.executionFrame.DoF()):] + interpSub, err = wf.localizationFrame.Interpolate(localizationFrameFromSubset, localizationFrameFromSubset, by) if err != nil { return nil, err } From 06b395d968a560c984647ade557bf6e4174491d9 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 11 Jul 2024 14:06:04 -0400 Subject: [PATCH 089/126] simplify the wrapper frame struct to remove seedmap + fs --- services/motion/builtin/move_request.go | 5 +- services/motion/builtin/wrapperFrame.go | 66 +++++++------------------ 2 files changed, 20 insertions(+), 51 deletions(-) diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 2b45fa2a626..d3c8314bd39 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -787,10 +787,7 @@ func (ms *builtIn) createBaseMoveRequest( if err != nil { return nil, err } - wrapperFrameSeedMap := map[string][]referenceframe.Input{} - wrapperFrameSeedMap[executionFrame.Name()] = make([]referenceframe.Input, len(executionFrame.DoF())) - wrapperFrameSeedMap[localizationFrame.Name()] = make([]referenceframe.Input, len(localizationFrame.DoF())) - wf, err := newWrapperFrame(localizationFrame, executionFrame, wrapperFrameSeedMap, wrapperFS) + wf, err := newWrapperFrame(localizationFrame, executionFrame) if err != nil { return nil, err } diff --git a/services/motion/builtin/wrapperFrame.go b/services/motion/builtin/wrapperFrame.go index 0331ef1a1f5..81475a380c2 100644 --- a/services/motion/builtin/wrapperFrame.go +++ b/services/motion/builtin/wrapperFrame.go @@ -2,7 +2,6 @@ package builtin import ( "github.com/pkg/errors" - "go.uber.org/multierr" pb "go.viam.com/api/component/arm/v1" "go.viam.com/rdk/motionplan/tpspace" @@ -17,15 +16,11 @@ type wrapperFrame struct { name string localizationFrame referenceframe.Frame executionFrame referenceframe.Frame - seedMap map[string][]referenceframe.Input - fs referenceframe.FrameSystem ptgSolvers []tpspace.PTGSolver } func newWrapperFrame( localizationFrame, executionFrame referenceframe.Frame, - seedMap map[string][]referenceframe.Input, - fs referenceframe.FrameSystem, ) (referenceframe.Frame, error) { ptgFrame, ok := executionFrame.(tpspace.PTGProvider) if !ok { @@ -35,8 +30,6 @@ func newWrapperFrame( name: executionFrame.Name(), localizationFrame: localizationFrame, executionFrame: executionFrame, - seedMap: seedMap, - fs: fs, ptgSolvers: ptgFrame.PTGSolvers(), }, nil } @@ -51,12 +44,15 @@ func (wf *wrapperFrame) Transform(inputs []referenceframe.Input) (spatialmath.Po if len(inputs) != len(wf.DoF()) { return nil, referenceframe.NewIncorrectInputLengthError(len(inputs), len(wf.DoF())) } - pf := referenceframe.NewPoseInFrame(wf.Name(), spatialmath.NewZeroPose()) - tf, err := wf.fs.Transform(wf.sliceToMap(inputs), pf, referenceframe.World) + executionFramePose, err := wf.executionFrame.Transform(inputs[:len(wf.executionFrame.DoF())]) if err != nil { return nil, err } - return tf.(*referenceframe.PoseInFrame).Pose(), nil + localizationFramePose, err := wf.localizationFrame.Transform(inputs[len(wf.executionFrame.DoF()):]) + if err != nil { + return nil, err + } + return spatialmath.Compose(executionFramePose, localizationFramePose), nil } // Interpolate interpolates the given amount between the two sets of inputs. @@ -102,40 +98,26 @@ func (wf *wrapperFrame) Geometries(inputs []referenceframe.Input) (*referencefra if len(inputs) != len(wf.DoF()) { return nil, referenceframe.NewIncorrectInputLengthError(len(inputs), len(wf.DoF())) } - var errAll error - inputMap := wf.sliceToMap(inputs) sfGeometries := []spatialmath.Geometry{} - for _, fName := range wf.fs.FrameNames() { - f := wf.fs.Frame(fName) - if f == nil { - return nil, referenceframe.NewFrameMissingError(fName) - } - inputs, err := referenceframe.GetFrameInputs(f, inputMap) - if err != nil { - return nil, err - } - gf, err := f.Geometries(inputs) - if gf == nil { - // only propagate errors that result in nil geometry - multierr.AppendInto(&errAll, err) - continue - } - var tf referenceframe.Transformable - tf, err = wf.fs.Transform(inputMap, gf, referenceframe.World) - if err != nil { - return nil, err - } - sfGeometries = append(sfGeometries, tf.(*referenceframe.GeometriesInFrame).Geometries()...) + gf, err := wf.executionFrame.Geometries(make([]referenceframe.Input, len(wf.executionFrame.DoF()))) + if err != nil { + return nil, err } - return referenceframe.NewGeometriesInFrame(referenceframe.World, sfGeometries), errAll + 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(referenceframe.World, sfGeometries), nil } // DoF returns the number of degrees of freedom within a given frame. func (wf *wrapperFrame) DoF() []referenceframe.Limit { var limits []referenceframe.Limit - for _, name := range wf.fs.FrameNames() { - limits = append(limits, wf.fs.Frame(name).DoF()...) - } + limits = append(limits, wf.executionFrame.DoF()...) + limits = append(limits, wf.localizationFrame.DoF()...) return limits } @@ -162,16 +144,6 @@ func (wf *wrapperFrame) ProtobufFromInput(input []referenceframe.Input) *pb.Join return &pb.JointPositions{Values: n} } -func (wf *wrapperFrame) sliceToMap(inputSlice []referenceframe.Input) map[string][]referenceframe.Input { - inputs := map[string][]referenceframe.Input{} - for k, v := range wf.seedMap { - inputs[k] = v - } - inputs[wf.executionFrame.Name()] = inputSlice[:len(wf.executionFrame.DoF())] - inputs[wf.localizationFrame.Name()] = inputSlice[len(wf.executionFrame.DoF()):] - return inputs -} - func (wf *wrapperFrame) PTGSolvers() []tpspace.PTGSolver { return wf.ptgSolvers } From 29767fa71b0022496fa39e3a22b9e2510fdf7fc3 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 11 Jul 2024 15:04:39 -0400 Subject: [PATCH 090/126] uncomment sub-test of TestPositionalReplanning --- services/motion/builtin/builtin_test.go | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index e4c25e389e7..3b38f144963 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -2,6 +2,7 @@ package builtin import ( "context" + "fmt" "math" "strings" "testing" @@ -288,13 +289,13 @@ func TestPositionalReplanning(t *testing.T) { // expectedSuccess: false, // extra: map[string]interface{}{"replan_cost_factor": 0.01, "smooth_iter": 5}, // }, - // { - // name: "check we replan with a noisy sensor", - // noise: r3.Vector{Y: epsilonMM + 0.1}, - // expectedErr: fmt.Sprintf("exceeded maximum number of replans: %d: plan failed", 4), - // expectedSuccess: false, - // extra: map[string]interface{}{"replan_cost_factor": 10.0, "max_replans": 4, "smooth_iter": 5}, - // }, + { + name: "check we replan with a noisy sensor", + noise: r3.Vector{Y: epsilonMM + 0.1}, + expectedErr: fmt.Sprintf("exceeded maximum number of replans: %d: plan failed", 4), + expectedSuccess: false, + extra: map[string]interface{}{"replan_cost_factor": 10.0, "max_replans": 4, "smooth_iter": 5}, + }, } testFn := func(t *testing.T, tc testCase) { From 7f84f39c6b74fd0c31691ad532836ef0e49fa9f0 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 11 Jul 2024 15:05:29 -0400 Subject: [PATCH 091/126] make inputsToPose private --- referenceframe/frame.go | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 33c014a0c1d..e20a53db0fe 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -482,7 +482,7 @@ func (pf *poseFrame) Transform(inputs []Input) (spatial.Pose, error) { if err := pf.baseFrame.validInputs(inputs); err != nil { return nil, NewIncorrectInputLengthError(len(inputs), 7) } - return InputsToPose(inputs), nil + return inputsToPose(inputs), nil } // Interpolate interpolates the given amount between the two sets of inputs. @@ -562,8 +562,8 @@ func PoseToInputs(p spatial.Pose) []Input { }) } -// InputsToPose is a convience method for turning inputs into a spatial.Pose. -func InputsToPose(inputs []Input) spatial.Pose { +// inputsToPose is a convience method for turning inputs into a spatial.Pose. +func inputsToPose(inputs []Input) spatial.Pose { return spatial.NewPose( r3.Vector{X: inputs[0].Value, Y: inputs[1].Value, Z: inputs[2].Value}, &spatial.OrientationVector{ From 1346d57e2c9a46d66856048bc1022458dbe4c569 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 11 Jul 2024 15:24:00 -0400 Subject: [PATCH 092/126] set theta to inf opposed to pi as conversion to quat will normalize it --- referenceframe/frame.go | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index e20a53db0fe..6525bf4e363 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -458,7 +458,7 @@ func NewPoseFrame(name string, geometry []spatial.Geometry) (Frame, error) { OX: math.Inf(1), OY: math.Inf(1), OZ: math.Inf(1), - Theta: math.Pi, + Theta: math.Inf(1), } orientationVector.Normalize() limits := []Limit{ From 5e5dbd6635cbe6afdd1287321f958d64af6abbfa Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 11 Jul 2024 15:27:49 -0400 Subject: [PATCH 093/126] Trigger Build From 32bf8991706a936075d3f2e644224ddc3a203489 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 11 Jul 2024 15:51:20 -0400 Subject: [PATCH 094/126] update TestObstacleReplanningSlam --- services/motion/builtin/builtin_test.go | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 3b38f144963..b71c39cf4bc 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -408,7 +408,11 @@ func TestObstacleReplanningSlam(t *testing.T) { ExecutionID: executionID, LastPlanOnly: true, }) - test.That(t, err, test.ShouldBeNil) + if err != nil { + test.That(t, strings.Contains(err.Error(), motion.ErrGoalWithinPlanDeviation.Error()), test.ShouldBeTrue) + } else { + test.That(t, err, test.ShouldBeNil) + } } func TestMultiplePieces(t *testing.T) { From e696e73401ae34fdc8cc94e063cd98e9c35d969a Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 11 Jul 2024 16:04:48 -0400 Subject: [PATCH 095/126] make error message correct --- services/motion/builtin/wrapperFrame.go | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/services/motion/builtin/wrapperFrame.go b/services/motion/builtin/wrapperFrame.go index 81475a380c2..4ac8dbfc942 100644 --- a/services/motion/builtin/wrapperFrame.go +++ b/services/motion/builtin/wrapperFrame.go @@ -24,7 +24,7 @@ func newWrapperFrame( ) (referenceframe.Frame, error) { ptgFrame, ok := executionFrame.(tpspace.PTGProvider) if !ok { - return nil, errors.New("cannot type assert localizationFrame into a tpspace.PTGProvider") + return nil, errors.New("cannot type assert executionFrame into a tpspace.PTGProvider") } return &wrapperFrame{ name: executionFrame.Name(), From 0d6cf0ea1c8355436556984b49b97ae22c0c332c Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 12 Jul 2024 13:45:39 -0400 Subject: [PATCH 096/126] remove ptgDoFLenght var from check.go thus removing special casing for plans --- motionplan/check.go | 19 +++---------------- motionplan/plan.go | 11 ++++++++--- services/motion/builtin/builtin_test.go | 19 +++++++++---------- services/motion/builtin/wrapperFrame.go | 5 ++--- 4 files changed, 22 insertions(+), 32 deletions(-) diff --git a/motionplan/check.go b/motionplan/check.go index af7e3c417bf..95c87fa8f8f 100644 --- a/motionplan/check.go +++ b/motionplan/check.go @@ -14,10 +14,7 @@ import ( "go.viam.com/rdk/spatialmath" ) -var ( - errCheckFrameNotInPath = errors.New("checkFrame given not in plan.Path() map") - ptgDoFLen = 4 -) +var errCheckFrameNotInPath = errors.New("checkFrame given not in plan.Path() map") // 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 @@ -233,18 +230,8 @@ func checkPlanRelative( return err } thisArcEndPose := spatialmath.Compose(thisArcEndPoseInWorld.Pose(), errorState) - startInputs := map[string][]referenceframe.Input{} - for k, v := range plan.Trajectory()[i] { - if k == checkFrame.Name() { - // Starting inputs for relative frames should be all-zero - correctedInputs := make([]referenceframe.Input, ptgDoFLen) - correctedInputs = append(correctedInputs, v[ptgDoFLen:]...) - startInputs[k] = correctedInputs - } else { - startInputs[k] = v - } - } - nextInputs := plan.Trajectory()[i] + startInputs := plan.Trajectory()[i] + nextInputs := startInputs segment, err := createSegment(sf, lastArcEndPose, thisArcEndPose, startInputs, nextInputs) if err != nil { return err diff --git a/motionplan/plan.go b/motionplan/plan.go index 2600e0f9fd0..8efbbb87d8f 100644 --- a/motionplan/plan.go +++ b/motionplan/plan.go @@ -313,11 +313,11 @@ func (e *ExecutionState) CurrentPoses() map[string]*referenceframe.PoseInFrame { func CalculateFrameErrorState(e ExecutionState, executionFrame, localizationFrame referenceframe.Frame) (spatialmath.Pose, error) { currentInputs, ok := e.CurrentInputs()[executionFrame.Name()] if !ok { - return nil, fmt.Errorf("1could not find frame %s in ExecutionState", executionFrame.Name()) + return nil, newFrameNotFoundError(executionFrame.Name()) } currentPose, ok := e.CurrentPoses()[localizationFrame.Name()] if !ok { - return nil, fmt.Errorf("2could not find frame %s in ExecutionState", localizationFrame.Name()) + return nil, newFrameNotFoundError(localizationFrame.Name()) } currPoseInArc, err := executionFrame.Transform(currentInputs) if err != nil { @@ -336,7 +336,7 @@ func CalculateFrameErrorState(e ExecutionState, executionFrame, localizationFram } pose, ok := path[index][executionFrame.Name()] if !ok { - return nil, fmt.Errorf("could not find frame %s in ExecutionState", executionFrame.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, executionFrame, localizationFram 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/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index b71c39cf4bc..e79c2432526 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -1062,7 +1062,6 @@ func TestCheckPlan(t *testing.T) { test.That(t, strings.Contains(err.Error(), "found constraint violation or collision in segment between"), test.ShouldBeTrue) }) - remainingPlan, err := motionplan.RemainingPlan(plan, 2) currentInputs = map[string][]referenceframe.Input{ mr.kinematicBase.Kinematics().Name(): { {Value: 0}, // ptg index @@ -1071,24 +1070,24 @@ func TestCheckPlan(t *testing.T) { {Value: 0}, // end distace along trajectory index }, mr.kinematicBase.LocalizationFrame().Name(): { - {Value: 2727.25}, // X - {Value: 0}, // Y - {Value: 0}, // Z - {Value: 0}, // OX - {Value: 0}, // OY - {Value: 1}, // OZ - {Value: -90}, // Theta + {Value: 2779.937}, // X + {Value: 0}, // Y + {Value: 0}, // Z + {Value: 0}, // OX + {Value: 0}, // OY + {Value: 1}, // OZ + {Value: -90}, // 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: 2727.25, Y: 0, Z: 0}, + r3.Vector{X: 2779.937, Y: 0, Z: 0}, &spatialmath.OrientationVectorDegrees{OZ: 1, Theta: -90}, )), } - newExecutionState, err := motionplan.NewExecutionState(remainingPlan, 0, currentInputs, currentPoses) + 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) diff --git a/services/motion/builtin/wrapperFrame.go b/services/motion/builtin/wrapperFrame.go index 4ac8dbfc942..e280e154358 100644 --- a/services/motion/builtin/wrapperFrame.go +++ b/services/motion/builtin/wrapperFrame.go @@ -11,7 +11,6 @@ import ( ) // WrapperFrame is a frame which merges the planning and localization frames of a PTG base. -// This struct is used so that we do not break abstractions made in CheckPlan. type wrapperFrame struct { name string localizationFrame referenceframe.Frame @@ -52,7 +51,7 @@ func (wf *wrapperFrame) Transform(inputs []referenceframe.Input) (spatialmath.Po if err != nil { return nil, err } - return spatialmath.Compose(executionFramePose, localizationFramePose), nil + return spatialmath.Compose(localizationFramePose, executionFramePose), nil } // Interpolate interpolates the given amount between the two sets of inputs. @@ -70,7 +69,7 @@ func (wf *wrapperFrame) Interpolate(from, to []referenceframe.Input, by float64) // the latter seven values correspond to pose(localization) frame values we want the interpolate // executionFrame interpolation - executionFrameFromSubset := from[:len(wf.executionFrame.DoF())] + 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 { From 5704cbdcfc789734372e46abcfd94a5a5bbe1000 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 12 Jul 2024 14:03:34 -0400 Subject: [PATCH 097/126] revert TestObstacleReplanningSlam --- services/motion/builtin/builtin_test.go | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index e79c2432526..f7e490c31f0 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -408,11 +408,7 @@ func TestObstacleReplanningSlam(t *testing.T) { ExecutionID: executionID, LastPlanOnly: true, }) - if err != nil { - test.That(t, strings.Contains(err.Error(), motion.ErrGoalWithinPlanDeviation.Error()), test.ShouldBeTrue) - } else { - test.That(t, err, test.ShouldBeNil) - } + test.That(t, err, test.ShouldBeNil) } func TestMultiplePieces(t *testing.T) { From 9fbb717166186aa17059c5f6a51e1e251e941bd3 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 18 Jul 2024 14:01:37 -0400 Subject: [PATCH 098/126] peter's requested changes, i.e. have next inputs not match startinginputs, make augmentBaseExecutionState robust again remaining plans, update wrapperFrame interpolation logic to only use 'to' values --- motionplan/check.go | 8 +++--- services/motion/builtin/move_request.go | 35 ++++++++++++++++++++----- services/motion/builtin/wrapperFrame.go | 8 ++---- 3 files changed, 35 insertions(+), 16 deletions(-) diff --git a/motionplan/check.go b/motionplan/check.go index 95c87fa8f8f..184eb093d81 100644 --- a/motionplan/check.go +++ b/motionplan/check.go @@ -16,6 +16,8 @@ import ( 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. @@ -141,7 +143,7 @@ func checkPlanRelative( return err } // change from 60mm to 30mm so we have finer interpolation along segments - sfPlanner.planOpts.Resolution = 30 + sfPlanner.planOpts.Resolution = relativePlanOptsResolution currentInputs := executionState.CurrentInputs() wayPointIdx := executionState.Index() @@ -230,8 +232,8 @@ func checkPlanRelative( return err } thisArcEndPose := spatialmath.Compose(thisArcEndPoseInWorld.Pose(), errorState) - startInputs := plan.Trajectory()[i] - nextInputs := startInputs + startInputs := plan.Trajectory()[i-1] + nextInputs := plan.Trajectory()[i] segment, err := createSegment(sf, lastArcEndPose, thisArcEndPose, startInputs, nextInputs) if err != nil { return err diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index d3c8314bd39..087743def1a 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -326,7 +326,7 @@ func (mr *moveRequest) obstaclesIntersectPlan( // 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 -// kinematicBase.Kinematics. +// mr.kinematicBase.Kinematics(). func (mr *moveRequest) augmentBaseExecutionState( baseExecutionState motionplan.ExecutionState, ) (motionplan.ExecutionState, error) { @@ -334,14 +334,35 @@ func (mr *moveRequest) augmentBaseExecutionState( existingPlan := baseExecutionState.Plan() newTrajectory := make(motionplan.Trajectory, 0, len(existingPlan.Trajectory())) for idx, currTraj := range existingPlan.Trajectory() { - if idx != 0 { - idx-- - } + // 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. + currPathStep := existingPlan.Path()[idx] kbPose := currPathStep[mr.kinematicBase.Kinematics().Name()] kbTraj := currTraj[mr.kinematicBase.Name().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(kbPose.Pose())...) + updatedTraj = append(updatedTraj, referenceframe.PoseToInputs(prevPathPose)...) newTrajectory = append( newTrajectory, map[string][]referenceframe.Input{mr.kinematicBase.Kinematics().Name(): updatedTraj}, ) @@ -570,8 +591,8 @@ func (ms *builtIn) newMoveOnGlobeRequest( return nil, fmt.Errorf("cannot move more than %d kilometers", int(maxTravelDistanceMM*1e-6)) } - // these limits need to be updated so that they are in 7DOF - // what is a good limit for what OX, OY, OZ should be? + // 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}, diff --git a/services/motion/builtin/wrapperFrame.go b/services/motion/builtin/wrapperFrame.go index e280e154358..2d5583ca512 100644 --- a/services/motion/builtin/wrapperFrame.go +++ b/services/motion/builtin/wrapperFrame.go @@ -82,12 +82,8 @@ func (wf *wrapperFrame) Interpolate(from, to []referenceframe.Input, by float64) // the ToSubset of the localizationFrame does not matter since the executionFrame interpolations // move us through a given segment and the localizationFrame input values are what we compose with // the position ourselves in our absolute position in world. - localizationFrameFromSubset := from[len(wf.executionFrame.DoF()):] - interpSub, err = wf.localizationFrame.Interpolate(localizationFrameFromSubset, localizationFrameFromSubset, by) - if err != nil { - return nil, err - } - interp = append(interp, interpSub...) + localizationFrameSubset := to[len(wf.executionFrame.DoF()):] + interp = append(interp, localizationFrameSubset...) return interp, nil } From 7cbe84ee6cba31a7bf4d6f55f2c7ec7fbb9ddb75 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 18 Jul 2024 15:33:20 -0400 Subject: [PATCH 099/126] add comment to explicitly state that the order of inputs for the poseFrame matters in code for augmentBaseExecutionState --- services/motion/builtin/move_request.go | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 087743def1a..c5e110c0c03 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -372,6 +372,12 @@ func (mr *moveRequest) augmentBaseExecutionState( // 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())..., From c7ae603e2ebc14358a344792f9938ad7b2641476 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 18 Jul 2024 17:12:42 -0400 Subject: [PATCH 100/126] remove validateRelPiF from check.go + have wrapper frames gifs be in their name instead of in world --- motionplan/check.go | 36 ------------------------- services/motion/builtin/wrapperFrame.go | 3 ++- 2 files changed, 2 insertions(+), 37 deletions(-) diff --git a/motionplan/check.go b/motionplan/check.go index 184eb093d81..bcdf703b0b1 100644 --- a/motionplan/check.go +++ b/motionplan/check.go @@ -70,38 +70,6 @@ func checkPlanRelative( lookAheadDistanceMM float64, sfPlanner *planManager, ) error { - // 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) { transformable, err := fs.Transform(inputs, pif, referenceframe.World) if err != nil { @@ -173,10 +141,6 @@ func checkPlanRelative( if !ok { return errors.New("checkFrame not found in current pose map") } - err = validateRelPiF(currentPoseIF) - if err != nil { - return err - } currentPoseInWorld, err := toWorld(currentPoseIF, currentInputs) if err != nil { return err diff --git a/services/motion/builtin/wrapperFrame.go b/services/motion/builtin/wrapperFrame.go index 2d5583ca512..8f1a93c1838 100644 --- a/services/motion/builtin/wrapperFrame.go +++ b/services/motion/builtin/wrapperFrame.go @@ -105,7 +105,8 @@ func (wf *wrapperFrame) Geometries(inputs []referenceframe.Input) (*referencefra for _, g := range gf.Geometries() { sfGeometries = append(sfGeometries, g.Transform(transformBy)) } - return referenceframe.NewGeometriesInFrame(referenceframe.World, sfGeometries), nil + + return referenceframe.NewGeometriesInFrame(wf.name, sfGeometries), nil } // DoF returns the number of degrees of freedom within a given frame. From 4f7652386ae21f5be4e415532f00dc957bdc994b Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 24 Jul 2024 11:41:21 -0400 Subject: [PATCH 101/126] make warn log correct so that we know what the base name is --- components/base/kinematicbase/differentialDrive.go | 2 +- components/base/kinematicbase/ptgKinematics.go | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/components/base/kinematicbase/differentialDrive.go b/components/base/kinematicbase/differentialDrive.go index f733ab999f6..e68122282e2 100644 --- a/components/base/kinematicbase/differentialDrive.go +++ b/components/base/kinematicbase/differentialDrive.go @@ -71,7 +71,7 @@ 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.CWarn(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 diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index f3fb1217583..006d034bb72 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -98,7 +98,7 @@ 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.CWarn(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 From cab355e1295ddbadd46153260b3f9587a9ba3f84 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 24 Jul 2024 11:55:32 -0400 Subject: [PATCH 102/126] fix lll --- components/base/kinematicbase/differentialDrive.go | 5 ++++- components/base/kinematicbase/ptgKinematics.go | 5 ++++- 2 files changed, 8 insertions(+), 2 deletions(-) diff --git a/components/base/kinematicbase/differentialDrive.go b/components/base/kinematicbase/differentialDrive.go index e68122282e2..4bcc3258531 100644 --- a/components/base/kinematicbase/differentialDrive.go +++ b/components/base/kinematicbase/differentialDrive.go @@ -71,7 +71,10 @@ 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.", b.Name().Name) + logger.CWarn( + 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 diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index 006d034bb72..7e95fb122b8 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -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.", b.Name().Name) + logger.CWarn( + 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 From 78dfab686bd38778b09b78fc2ebdc6abef33b3dc Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 25 Jul 2024 13:37:28 -0400 Subject: [PATCH 103/126] have move on globe origin not reset to zero when we replan --- services/motion/builtin/builtin.go | 2 ++ services/motion/builtin/move_request.go | 39 ++++++++++++------------- 2 files changed, 20 insertions(+), 21 deletions(-) diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index 33c0bbea02f..d2e8367027f 100644 --- a/services/motion/builtin/builtin.go +++ b/services/motion/builtin/builtin.go @@ -9,6 +9,7 @@ import ( "github.com/golang/geo/r3" "github.com/google/uuid" + geo "github.com/kellydunn/golang-geo" "github.com/pkg/errors" "go.viam.com/rdk/components/movementsensor" @@ -159,6 +160,7 @@ type builtIn struct { components map[resource.Name]resource.Resource logger logging.Logger state *state.State + globeOrigin *geo.Point } func (ms *builtIn) Close(ctx context.Context) error { diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index c5e110c0c03..05bced1b83c 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -183,6 +183,7 @@ func (mr *moveRequest) deviatedFromPlan(ctx context.Context, plan motionplan.Pla // getTransientDetections returns a list of geometries as observed by the provided vision service and camera. // Depending on the caller, the geometries returned are either in their relative position // with respect to the base or in their absolute position with respect to the world. +// TODO(RSDK-8340): Explore usefulness of pointcloud instead of spatialmath.Geometry. func (mr *moveRequest) getTransientDetections( ctx context.Context, visSrvc vision.Service, @@ -552,9 +553,14 @@ func (ms *builtIn) newMoveOnGlobeRequest( if !ok { return nil, resource.DependencyNotFoundError(req.MovementSensorName) } - origin, _, err := movementSensor.Position(ctx, nil) - if err != nil { - return nil, err + + // if we have some previous origin we've already recorded, + // use that instead of resetting the origin + if ms.globeOrigin == nil { + ms.globeOrigin, _, err = movementSensor.Position(ctx, nil) + if err != nil { + return nil, err + } } heading, err := movementSensor.CompassHeading(ctx, nil) @@ -570,7 +576,7 @@ func (ms *builtIn) newMoveOnGlobeRequest( movementSensorToBase = baseOrigin } // Create a localizer from the movement sensor, and collapse reported orientations to 2d - localizer := motion.TwoDLocalizer(motion.NewMovementSensorLocalizer(movementSensor, origin, movementSensorToBase.Pose())) + localizer := motion.TwoDLocalizer(motion.NewMovementSensorLocalizer(movementSensor, ms.globeOrigin, movementSensorToBase.Pose())) // create a KinematicBase from the componentName baseComponent, ok := ms.components[req.ComponentName] @@ -590,7 +596,7 @@ func (ms *builtIn) newMoveOnGlobeRequest( // Important: GeoPointToPose will create a pose such that incrementing latitude towards north increments +Y, and incrementing // longitude towards east increments +X. Heading is not taken into account. This pose must therefore be transformed based on the // orientation of the base such that it is a pose relative to the base's current location. - goalPoseRaw := spatialmath.NewPoseFromPoint(spatialmath.GeoPointToPoint(req.Destination, origin)) + goalPoseRaw := spatialmath.NewPoseFromPoint(spatialmath.GeoPointToPoint(req.Destination, ms.globeOrigin)) // construct limits straightlineDistance := goalPoseRaw.Point().Norm() if straightlineDistance > maxTravelDistanceMM { @@ -610,10 +616,10 @@ func (ms *builtIn) newMoveOnGlobeRequest( } // convert obstacles of type []GeoGeometry into []Geometry - geomsRaw := spatialmath.GeoGeometriesToGeometries(obstacles, origin) + geomsRaw := spatialmath.GeoGeometriesToGeometries(obstacles, ms.globeOrigin) // convert bounding regions which are GeoGeometries into Geometries - boundingRegions := spatialmath.GeoGeometriesToGeometries(req.BoundingRegions, origin) + boundingRegions := spatialmath.GeoGeometriesToGeometries(req.BoundingRegions, ms.globeOrigin) mr, err := ms.createBaseMoveRequest( ctx, @@ -631,7 +637,7 @@ func (ms *builtIn) newMoveOnGlobeRequest( mr.seedPlan = seedPlan mr.replanCostFactor = valExtra.replanCostFactor mr.requestType = requestTypeMoveOnGlobe - mr.geoPoseOrigin = spatialmath.NewGeoPose(origin, heading) + mr.geoPoseOrigin = spatialmath.NewGeoPose(ms.globeOrigin, heading) mr.planRequest.BoundingRegions = boundingRegions return mr, nil } @@ -799,21 +805,12 @@ func (ms *builtIn) createBaseMoveRequest( } } - // We create a wrapperFrame and a collision framesystem 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. + // 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() - wrapperFS := referenceframe.NewEmptyFrameSystem("wrapperFS") - err = wrapperFS.AddFrame(localizationFrame, wrapperFS.World()) - if err != nil { - return nil, err - } - err = wrapperFS.AddFrame(executionFrame, localizationFrame) - if err != nil { - return nil, err - } wf, err := newWrapperFrame(localizationFrame, executionFrame) if err != nil { return nil, err From 6b4c1ff8690c0353c76386a5ad2067a441ad89f5 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 26 Jul 2024 15:36:08 -0400 Subject: [PATCH 104/126] change from CWarn to CWarnf so that we can get the base's name printed correctly --- components/base/kinematicbase/differentialDrive.go | 2 +- components/base/kinematicbase/ptgKinematics.go | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/components/base/kinematicbase/differentialDrive.go b/components/base/kinematicbase/differentialDrive.go index 4bcc3258531..8f8ff7231e4 100644 --- a/components/base/kinematicbase/differentialDrive.go +++ b/components/base/kinematicbase/differentialDrive.go @@ -71,7 +71,7 @@ func wrapWithDifferentialDriveKinematics( boundingSphere, err = spatialmath.BoundingSphere(geometry) } if boundingSphere == nil || err != nil { - logger.CWarn( + logger.CWarnf( ctx, "base %s not configured with a geometry, will be considered a 300mm sphere for collision detection purposes.", b.Name().Name, ) diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index 7e95fb122b8..4be6436de63 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -98,7 +98,7 @@ func wrapWithPTGKinematics( geometries, err := b.Geometries(ctx, nil) if len(geometries) == 0 || err != nil { - logger.CWarn( + logger.CWarnf( ctx, "base %s not configured with a geometry, will be considered a 300mm sphere for collision detection purposes.", b.Name().Name, ) @@ -192,7 +192,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 } From ece913bcca87b5b1ce22afe409f3f1ca73e16458 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 31 Jul 2024 11:28:50 -0400 Subject: [PATCH 105/126] have actualPiF be converted to have its theta be in radians --- components/base/kinematicbase/ptgKinematics.go | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index 4be6436de63..7cb06a0022b 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -196,6 +196,10 @@ func (ptgk *ptgBaseKinematics) ExecutionState(ctx context.Context) (motionplan.E if err != nil { return motionplan.ExecutionState{}, err } + actualPIFInRadians := referenceframe.NewPoseInFrame( + actualPIF.Parent(), + spatialmath.NewPose(actualPIF.Pose().Point(), actualPIF.Pose().Orientation().OrientationVectorRadians()), + ) ptgk.inputLock.RLock() currentIdx := ptgk.currentState.currentIdx @@ -208,7 +212,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.LocalizationFrame().Name(): actualPIF}, + map[string]*referenceframe.PoseInFrame{ptgk.LocalizationFrame().Name(): actualPIFInRadians}, ) } From 2887e02c7884696c3bf92bab3f950e50f7615541 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 31 Jul 2024 11:31:07 -0400 Subject: [PATCH 106/126] update nomencalture to be more descriptive --- referenceframe/frame.go | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 6525bf4e363..b2921d82675 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -502,7 +502,7 @@ func (pf *poseFrame) Interpolate(from, to []Input, by float64) ([]Input, error) return nil, err } interpolatedPose := spatial.Interpolate(fromPose, toPose, by) - return PoseToInputs(interpolatedPose), nil + return PoseToInputsRadians(interpolatedPose), nil } // Geometries returns an object representing the 3D space associeted with the staticFrame. @@ -549,10 +549,10 @@ func (pf *poseFrame) ProtobufFromInput(input []Input) *pb.JointPositions { return &pb.JointPositions{Values: n} } -// PoseToInputs is a convience method for turning poses into inputs +// PoseToInputsRadians is a convience method for turning poses into inputs // We note that the orientation of the pose will be understood // as OrientationVectorRadians. -func PoseToInputs(p spatial.Pose) []Input { +func PoseToInputsRadians(p spatial.Pose) []Input { return FloatsToInputs([]float64{ p.Point().X, p.Point().Y, p.Point().Z, p.Orientation().OrientationVectorRadians().OX, @@ -562,7 +562,8 @@ func PoseToInputs(p spatial.Pose) []Input { }) } -// inputsToPose is a convience method for turning inputs into a spatial.Pose. +// inputsToPose is a convience method on the poseFrame for turning inputs into a pose. +// We expect the inputs orientation's theta to be in radians func inputsToPose(inputs []Input) spatial.Pose { return spatial.NewPose( r3.Vector{X: inputs[0].Value, Y: inputs[1].Value, Z: inputs[2].Value}, From 0ad5c887db0dbb1fea63fcb173fe3e45628209c1 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 31 Jul 2024 11:34:34 -0400 Subject: [PATCH 107/126] rename to use PoseToInputsRadians --- services/motion/builtin/move_request.go | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 05bced1b83c..71f67e12109 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -204,7 +204,7 @@ func (mr *moveRequest) getTransientDetections( return nil, err } kbInputs := make([]referenceframe.Input, len(mr.kinematicBase.Kinematics().DoF())) - kbInputs = append(kbInputs, referenceframe.PoseToInputs( + kbInputs = append(kbInputs, referenceframe.PoseToInputsRadians( baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose(), )...) inputMap[mr.kinematicBase.Name().ShortName()] = kbInputs @@ -363,7 +363,7 @@ func (mr *moveRequest) augmentBaseExecutionState( } prevPathPose := spatialmath.PoseBetweenInverse(trajPose, kbPose.Pose()) updatedTraj := kbTraj - updatedTraj = append(updatedTraj, referenceframe.PoseToInputs(prevPathPose)...) + updatedTraj = append(updatedTraj, referenceframe.PoseToInputsRadians(prevPathPose)...) newTrajectory = append( newTrajectory, map[string][]referenceframe.Input{mr.kinematicBase.Kinematics().Name(): updatedTraj}, ) @@ -381,7 +381,7 @@ func (mr *moveRequest) augmentBaseExecutionState( // X, Y, Z, OX, OY, OZ, Theta (in radians) kinematicBaseCurrentInputs = append( kinematicBaseCurrentInputs, - referenceframe.PoseToInputs(baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose())..., + referenceframe.PoseToInputsRadians(baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose())..., ) allCurrentInputsFromBaseExecutionState[mr.kinematicBase.Kinematics().Name()] = kinematicBaseCurrentInputs From 9409384e01f788d3049d18519aa431ffbf75b88d Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 31 Jul 2024 11:43:39 -0400 Subject: [PATCH 108/126] add TestGetTransientDetectionsMath + have orientation of camera mimic the orientation on an intel realsense + update positions and dimensions for TestObstacleReplanningGlobe --- services/motion/builtin/builtin_test.go | 123 +++++++++++++++++- services/motion/builtin/move_on_globe_test.go | 12 +- services/motion/builtin/testutils.go | 4 +- 3 files changed, 131 insertions(+), 8 deletions(-) diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index f7e490c31f0..c5e191b0c9c 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -782,7 +782,7 @@ func TestStoppableMoveFunctions(t *testing.T) { }) } -func TestGetTransientDetections(t *testing.T) { +func TestGetTransientDetectionsSlam(t *testing.T) { ctx := context.Background() _, ms, closeFunc := CreateMoveOnMapTestEnvironment( @@ -855,6 +855,125 @@ func TestGetTransientDetections(t *testing.T) { } } +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())) + localizationFramePoseRadians := spatialmath.NewPose( + currentPose.Point(), currentPose.Orientation().OrientationVectorRadians(), + ) + kbInputs = append(kbInputs, referenceframe.PoseToInputsRadians(localizationFramePoseRadians)...) + 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) + fmt.Println("inputMap: ", inputMap) + + 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: "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: "base slighly 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: "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: "base slighly 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 := 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) { + testFn(t, c) + }) + } +} + func TestStopPlan(t *testing.T) { ctx := context.Background() ctx, cFunc := context.WithCancel(ctx) @@ -1072,7 +1191,7 @@ func TestCheckPlan(t *testing.T) { {Value: 0}, // OX {Value: 0}, // OY {Value: 1}, // OZ - {Value: -90}, // Theta + {Value: -math.Pi}, // Theta }, cameraFrame.Name(): referenceframe.FloatsToInputs(make([]float64, len(cameraFrame.DoF()))), } diff --git a/services/motion/builtin/move_on_globe_test.go b/services/motion/builtin/move_on_globe_test.go index 41e43b62d6d..5a28032feb6 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: 40, Y: 40, Z: 10}, caseName) + // NOTE: the orientation of the camera is OY:1, Theta: -90 degrees, this influences both the positon 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()) 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, ) From 26d956f4d9a1f29b72c5b5f8eafa6397bbd55ee3 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 31 Jul 2024 12:29:16 -0400 Subject: [PATCH 109/126] linted --- referenceframe/frame.go | 2 +- services/motion/builtin/builtin_test.go | 5 ++--- services/motion/builtin/move_on_globe_test.go | 2 +- 3 files changed, 4 insertions(+), 5 deletions(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index b2921d82675..65ea1e037ad 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -563,7 +563,7 @@ func PoseToInputsRadians(p spatial.Pose) []Input { } // inputsToPose is a convience method on the poseFrame for turning inputs into a pose. -// We expect the inputs orientation's theta to be in radians +// We expect the inputs orientation's theta to be in radians. func inputsToPose(inputs []Input) spatial.Pose { return spatial.NewPose( r3.Vector{X: inputs[0].Value, Y: inputs[1].Value, Z: inputs[2].Value}, diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index c5e191b0c9c..b83c4e1d20c 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -906,7 +906,6 @@ func TestGetTransientDetectionsMath(t *testing.T) { cam, ok := ms.(*builtIn).components[resource.NewName(camera.API, "injectedCamera")] test.That(t, ok, test.ShouldBeTrue) - fmt.Println("inputMap: ", inputMap) tf, err := mr.localizingFS.Transform( inputMap, @@ -935,7 +934,7 @@ func TestGetTransientDetectionsMath(t *testing.T) { expectedGeomPose: spatialmath.NewPose(r3.Vector{X: 10, Y: 1500, Z: 30}, &spatialmath.OrientationVectorDegrees{OY: 1, Theta: -90}), }, { - name: "base slighly from origin with zero theta", + 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}), @@ -949,7 +948,7 @@ func TestGetTransientDetectionsMath(t *testing.T) { ), }, { - name: "base slighly from origin with non-zero theta", + 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( diff --git a/services/motion/builtin/move_on_globe_test.go b/services/motion/builtin/move_on_globe_test.go index 5a28032feb6..f3675779f89 100644 --- a/services/motion/builtin/move_on_globe_test.go +++ b/services/motion/builtin/move_on_globe_test.go @@ -313,7 +313,7 @@ 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. - // NOTE: the orientation of the camera is OY:1, Theta: -90 degrees, this influences both the positon and dimensions of + // 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) From d154dce811d942121bb1f9a89b5ef271d4ebfcd9 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 31 Jul 2024 14:20:52 -0400 Subject: [PATCH 110/126] format segment string --- motionplan/ik/metrics.go | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/motionplan/ik/metrics.go b/motionplan/ik/metrics.go index 402ccec7f55..85af5766d7c 100644 --- a/motionplan/ik/metrics.go +++ b/motionplan/ik/metrics.go @@ -23,7 +23,7 @@ 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, From be344a45a9e4b4ace40172f08482135758d191a8 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 31 Jul 2024 14:54:45 -0400 Subject: [PATCH 111/126] update augmentBaseExecutionState --- services/motion/builtin/move_request.go | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 71f67e12109..4b2b8fc86ba 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -354,6 +354,9 @@ func (mr *moveRequest) augmentBaseExecutionState( // 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] kbPose := currPathStep[mr.kinematicBase.Kinematics().Name()] kbTraj := currTraj[mr.kinematicBase.Name().Name] @@ -361,7 +364,12 @@ func (mr *moveRequest) augmentBaseExecutionState( if err != nil { return baseExecutionState, err } - prevPathPose := spatialmath.PoseBetweenInverse(trajPose, kbPose.Pose()) + var prevPathPose spatialmath.Pose + if idx == baseExecutionState.Index() { + prevPathPose = baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose() + } else { + prevPathPose = spatialmath.PoseBetweenInverse(trajPose, kbPose.Pose()) + } updatedTraj := kbTraj updatedTraj = append(updatedTraj, referenceframe.PoseToInputsRadians(prevPathPose)...) newTrajectory = append( From 3862ac377634073b393568d812ef59988610681a Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 31 Jul 2024 15:22:30 -0400 Subject: [PATCH 112/126] fix lll --- motionplan/ik/metrics.go | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/motionplan/ik/metrics.go b/motionplan/ik/metrics.go index 85af5766d7c..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: \n\t 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, From b19e03218d331f19bbad3cdd799cadce20237b38 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Wed, 31 Jul 2024 23:30:48 -0400 Subject: [PATCH 113/126] add comments + re-arrange origin section of a segment in augmentBaseExecutionState --- services/motion/builtin/move_request.go | 24 ++++++++++++++++++------ services/motion/builtin/wrapperFrame.go | 18 +++++++++++++----- 2 files changed, 31 insertions(+), 11 deletions(-) diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 4b2b8fc86ba..593604b77d4 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -199,6 +199,9 @@ func (mr *moveRequest) getTransientDetections( 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 @@ -224,6 +227,10 @@ func (mr *moveRequest) getTransientDetections( label += "_" + geometry.Label() } geometry.SetLabel(label) + + // 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}), @@ -358,18 +365,21 @@ func (mr *moveRequest) augmentBaseExecutionState( // we will use the base's reported current position. currPathStep := existingPlan.Path()[idx] - kbPose := currPathStep[mr.kinematicBase.Kinematics().Name()] kbTraj := currTraj[mr.kinematicBase.Name().Name] - trajPose, err := mr.kinematicBase.Kinematics().Transform(kbTraj) - if err != nil { - return baseExecutionState, err - } + + // 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.PoseToInputsRadians(prevPathPose)...) newTrajectory = append( @@ -393,7 +403,9 @@ func (mr *moveRequest) augmentBaseExecutionState( ) allCurrentInputsFromBaseExecutionState[mr.kinematicBase.Kinematics().Name()] = kinematicBaseCurrentInputs - // update currentPoses + // 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 diff --git a/services/motion/builtin/wrapperFrame.go b/services/motion/builtin/wrapperFrame.go index 8f1a93c1838..700c09372ed 100644 --- a/services/motion/builtin/wrapperFrame.go +++ b/services/motion/builtin/wrapperFrame.go @@ -43,10 +43,13 @@ func (wf *wrapperFrame) Transform(inputs []referenceframe.Input) (spatialmath.Po 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 @@ -68,6 +71,9 @@ func (wf *wrapperFrame) Interpolate(from, to []referenceframe.Input, by float64) // 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())] @@ -77,11 +83,13 @@ func (wf *wrapperFrame) Interpolate(from, to []referenceframe.Input, by float64) } interp = append(interp, interpSub...) - // localizationFrame interpolation - // interpolating the localizationFrame is a special case - // the ToSubset of the localizationFrame does not matter since the executionFrame interpolations - // move us through a given segment and the localizationFrame input values are what we compose with - // the position ourselves in our absolute position in world. + // 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...) From 32bc72cfbc4a89be9b59fe373775393391dd5c52 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Thu, 1 Aug 2024 15:34:57 -0400 Subject: [PATCH 114/126] ray's requested changes --- .../base/kinematicbase/ptgKinematics.go | 3 +- referenceframe/frame.go | 34 ++++++++----------- services/motion/builtin/move_on_map_test.go | 14 ++++---- services/motion/builtin/move_request.go | 4 +-- services/motion/builtin/wrapperFrame.go | 12 +++---- 5 files changed, 29 insertions(+), 38 deletions(-) diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index 7cb06a0022b..57736497435 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -183,8 +183,7 @@ func (ptgk *ptgBaseKinematics) CurrentInputs(ctx context.Context) ([]referencefr ptgk.inputLock.RLock() defer ptgk.inputLock.RUnlock() - planningFrameInputs := ptgk.currentState.currentInputs - return planningFrameInputs, nil + return ptgk.currentState.currentInputs, nil } func (ptgk *ptgBaseKinematics) ExecutionState(ctx context.Context) (motionplan.ExecutionState, error) { diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 65ea1e037ad..52add6a3429 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -480,9 +480,17 @@ func NewPoseFrame(name string, geometry []spatial.Geometry) (Frame, error) { // 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, NewIncorrectInputLengthError(len(inputs), 7) + return nil, err } - return inputsToPose(inputs), nil + 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. @@ -535,7 +543,7 @@ func (pf *poseFrame) MarshalJSON() ([]byte, error) { func (pf *poseFrame) InputFromProtobuf(jp *pb.JointPositions) []Input { n := make([]Input, len(jp.Values)) for idx, d := range jp.Values { - n[idx] = Input{utils.DegToRad(d)} + n[idx] = Input{d} } return n } @@ -549,9 +557,9 @@ func (pf *poseFrame) ProtobufFromInput(input []Input) *pb.JointPositions { return &pb.JointPositions{Values: n} } -// PoseToInputsRadians is a convience method for turning poses into inputs -// We note that the orientation of the pose will be understood -// as OrientationVectorRadians. +// PoseToInputsRadians 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 PoseToInputsRadians(p spatial.Pose) []Input { return FloatsToInputs([]float64{ p.Point().X, p.Point().Y, p.Point().Z, @@ -561,17 +569,3 @@ func PoseToInputsRadians(p spatial.Pose) []Input { p.Orientation().OrientationVectorRadians().Theta, }) } - -// inputsToPose is a convience method on the poseFrame for turning inputs into a pose. -// We expect the inputs orientation's theta to be in radians. -func inputsToPose(inputs []Input) spatial.Pose { - 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, - }, - ) -} diff --git a/services/motion/builtin/move_on_map_test.go b/services/motion/builtin/move_on_map_test.go index 97a53252f9b..35e777616f8 100644 --- a/services/motion/builtin/move_on_map_test.go +++ b/services/motion/builtin/move_on_map_test.go @@ -284,13 +284,13 @@ func TestMoveOnMapStaticObs(t *testing.T) { {Value: 0}, // end distace along trajectory index }, mr.kinematicBase.LocalizationFrame().Name(): { - {Value: 587.720000000000027284841053}, // X - {Value: -808.259999999999990905052982}, // Y - {Value: 0}, // Z - {Value: 0}, // OX - {Value: 0}, // OY - {Value: 1}, // OZ - {Value: 0}, // Theta + {Value: 587}, // X + {Value: -808}, // Y + {Value: 0}, // Z + {Value: 0}, // OX + {Value: 0}, // OY + {Value: 1}, // OZ + {Value: 0}, // Theta }, } diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 593604b77d4..557c8106062 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -277,7 +277,7 @@ func (mr *moveRequest) obstaclesIntersectPlan( return state.ExecuteResponse{}, err } if len(gifs.Geometries()) == 0 { - mr.logger.CDebug(ctx, "will not check if obstacles intersect path since nothing was detected") + mr.logger.CDebug(ctx, "no obstacles detected") continue } @@ -326,7 +326,7 @@ 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 baseExectionState. +// 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 diff --git a/services/motion/builtin/wrapperFrame.go b/services/motion/builtin/wrapperFrame.go index 700c09372ed..846420e1124 100644 --- a/services/motion/builtin/wrapperFrame.go +++ b/services/motion/builtin/wrapperFrame.go @@ -18,12 +18,10 @@ type wrapperFrame struct { ptgSolvers []tpspace.PTGSolver } -func newWrapperFrame( - localizationFrame, executionFrame referenceframe.Frame, -) (referenceframe.Frame, error) { - ptgFrame, ok := executionFrame.(tpspace.PTGProvider) - if !ok { - return nil, errors.New("cannot type assert executionFrame into a tpspace.PTGProvider") +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(), @@ -134,7 +132,7 @@ func (wf *wrapperFrame) MarshalJSON() ([]byte, error) { func (wf *wrapperFrame) InputFromProtobuf(jp *pb.JointPositions) []referenceframe.Input { n := make([]referenceframe.Input, len(jp.Values)) for idx, d := range jp.Values { - n[idx] = referenceframe.Input{Value: utils.DegToRad(d)} + n[idx] = referenceframe.Input{Value: d} } return n } From 9dc7a18bbc18a27c5e506c7bd1e00e94f2bcda4f Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 2 Aug 2024 11:42:26 -0400 Subject: [PATCH 115/126] no need to have pose be in radians as returned by the kinematic's base execution frame since we handle the conversion elsewhere --- components/base/kinematicbase/ptgKinematics.go | 6 +----- services/motion/builtin/builtin_test.go | 5 +---- 2 files changed, 2 insertions(+), 9 deletions(-) diff --git a/components/base/kinematicbase/ptgKinematics.go b/components/base/kinematicbase/ptgKinematics.go index 57736497435..d9b121d9a59 100644 --- a/components/base/kinematicbase/ptgKinematics.go +++ b/components/base/kinematicbase/ptgKinematics.go @@ -195,10 +195,6 @@ func (ptgk *ptgBaseKinematics) ExecutionState(ctx context.Context) (motionplan.E if err != nil { return motionplan.ExecutionState{}, err } - actualPIFInRadians := referenceframe.NewPoseInFrame( - actualPIF.Parent(), - spatialmath.NewPose(actualPIF.Pose().Point(), actualPIF.Pose().Orientation().OrientationVectorRadians()), - ) ptgk.inputLock.RLock() currentIdx := ptgk.currentState.currentIdx @@ -211,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.LocalizationFrame().Name(): actualPIFInRadians}, + map[string]*referenceframe.PoseInFrame{ptgk.LocalizationFrame().Name(): actualPIF}, ) } diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index b83c4e1d20c..4d85105f2fa 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -893,10 +893,7 @@ func TestGetTransientDetectionsMath(t *testing.T) { inputMap, _, err := mr.fsService.CurrentInputs(ctx) test.That(t, err, test.ShouldBeNil) kbInputs := make([]referenceframe.Input, len(mr.kinematicBase.Kinematics().DoF())) - localizationFramePoseRadians := spatialmath.NewPose( - currentPose.Point(), currentPose.Orientation().OrientationVectorRadians(), - ) - kbInputs = append(kbInputs, referenceframe.PoseToInputsRadians(localizationFramePoseRadians)...) + kbInputs = append(kbInputs, referenceframe.PoseToInputsRadians(currentPose)...) inputMap[mr.kinematicBase.Name().ShortName()] = kbInputs fakeDetectionGeometry, err := spatialmath.NewBox( From f2ef264cebe46bfb44026900ab72db8ef75495ac Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 2 Aug 2024 12:06:12 -0400 Subject: [PATCH 116/126] rename PoseToInputsRadians to just be PoseToInputs with a detailed description of the method as a comment --- referenceframe/frame.go | 4 ++-- services/motion/builtin/builtin_test.go | 2 +- services/motion/builtin/move_request.go | 6 +++--- services/motion/builtin/wrapperFrame.go | 5 +---- 4 files changed, 7 insertions(+), 10 deletions(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 52add6a3429..0902924eda1 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -510,7 +510,7 @@ func (pf *poseFrame) Interpolate(from, to []Input, by float64) ([]Input, error) return nil, err } interpolatedPose := spatial.Interpolate(fromPose, toPose, by) - return PoseToInputsRadians(interpolatedPose), nil + return PoseToInputs(interpolatedPose), nil } // Geometries returns an object representing the 3D space associeted with the staticFrame. @@ -560,7 +560,7 @@ func (pf *poseFrame) ProtobufFromInput(input []Input) *pb.JointPositions { // PoseToInputsRadians 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 PoseToInputsRadians(p spatial.Pose) []Input { +func PoseToInputs(p spatial.Pose) []Input { return FloatsToInputs([]float64{ p.Point().X, p.Point().Y, p.Point().Z, p.Orientation().OrientationVectorRadians().OX, diff --git a/services/motion/builtin/builtin_test.go b/services/motion/builtin/builtin_test.go index 4d85105f2fa..eeee2272c1a 100644 --- a/services/motion/builtin/builtin_test.go +++ b/services/motion/builtin/builtin_test.go @@ -893,7 +893,7 @@ func TestGetTransientDetectionsMath(t *testing.T) { 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.PoseToInputsRadians(currentPose)...) + kbInputs = append(kbInputs, referenceframe.PoseToInputs(currentPose)...) inputMap[mr.kinematicBase.Name().ShortName()] = kbInputs fakeDetectionGeometry, err := spatialmath.NewBox( diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 557c8106062..275cf8f4fd4 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -207,7 +207,7 @@ func (mr *moveRequest) getTransientDetections( return nil, err } kbInputs := make([]referenceframe.Input, len(mr.kinematicBase.Kinematics().DoF())) - kbInputs = append(kbInputs, referenceframe.PoseToInputsRadians( + kbInputs = append(kbInputs, referenceframe.PoseToInputs( baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose(), )...) inputMap[mr.kinematicBase.Name().ShortName()] = kbInputs @@ -381,7 +381,7 @@ func (mr *moveRequest) augmentBaseExecutionState( } updatedTraj := kbTraj - updatedTraj = append(updatedTraj, referenceframe.PoseToInputsRadians(prevPathPose)...) + updatedTraj = append(updatedTraj, referenceframe.PoseToInputs(prevPathPose)...) newTrajectory = append( newTrajectory, map[string][]referenceframe.Input{mr.kinematicBase.Kinematics().Name(): updatedTraj}, ) @@ -399,7 +399,7 @@ func (mr *moveRequest) augmentBaseExecutionState( // X, Y, Z, OX, OY, OZ, Theta (in radians) kinematicBaseCurrentInputs = append( kinematicBaseCurrentInputs, - referenceframe.PoseToInputsRadians(baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose())..., + referenceframe.PoseToInputs(baseExecutionState.CurrentPoses()[mr.kinematicBase.LocalizationFrame().Name()].Pose())..., ) allCurrentInputsFromBaseExecutionState[mr.kinematicBase.Kinematics().Name()] = kinematicBaseCurrentInputs diff --git a/services/motion/builtin/wrapperFrame.go b/services/motion/builtin/wrapperFrame.go index 846420e1124..c339e8c738b 100644 --- a/services/motion/builtin/wrapperFrame.go +++ b/services/motion/builtin/wrapperFrame.go @@ -117,10 +117,7 @@ func (wf *wrapperFrame) Geometries(inputs []referenceframe.Input) (*referencefra // DoF returns the number of degrees of freedom within a given frame. func (wf *wrapperFrame) DoF() []referenceframe.Limit { - var limits []referenceframe.Limit - limits = append(limits, wf.executionFrame.DoF()...) - limits = append(limits, wf.localizationFrame.DoF()...) - return limits + return append(wf.executionFrame.DoF(), wf.localizationFrame.DoF()...) } // MarshalJSON serializes a given frame. From eeaa7c8cbf24f6d4864b5d49736fbcdb2eedf657 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 2 Aug 2024 12:13:21 -0400 Subject: [PATCH 117/126] fix PoseToInputs comment --- referenceframe/frame.go | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 0902924eda1..322e4a6d85b 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -557,7 +557,7 @@ func (pf *poseFrame) ProtobufFromInput(input []Input) *pb.JointPositions { return &pb.JointPositions{Values: n} } -// PoseToInputsRadians is a convenience method for turning a pose into a slice of inputs +// 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 { From 9419cc7886536a291ba87c6b87318217f1023afa Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 2 Aug 2024 13:38:35 -0400 Subject: [PATCH 118/126] move logic of keeping track or geoPose origin into the state package and have it be conveyed through extra --- services/motion/builtin/builtin.go | 2 -- services/motion/builtin/move_request.go | 31 +++++++++++++++---------- services/motion/builtin/state/state.go | 25 ++++++++++++++++++++ 3 files changed, 44 insertions(+), 14 deletions(-) diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index d2e8367027f..33c0bbea02f 100644 --- a/services/motion/builtin/builtin.go +++ b/services/motion/builtin/builtin.go @@ -9,7 +9,6 @@ import ( "github.com/golang/geo/r3" "github.com/google/uuid" - geo "github.com/kellydunn/golang-geo" "github.com/pkg/errors" "go.viam.com/rdk/components/movementsensor" @@ -160,7 +159,6 @@ type builtIn struct { components map[resource.Name]resource.Resource logger logging.Logger state *state.State - globeOrigin *geo.Point } func (ms *builtIn) Close(ctx context.Context) error { diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 275cf8f4fd4..6079babfdd9 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -9,6 +9,7 @@ import ( "sync" "time" + geo "github.com/kellydunn/golang-geo" "github.com/pkg/errors" goutils "go.viam.com/utils" @@ -26,6 +27,7 @@ import ( "go.viam.com/rdk/services/slam" "go.viam.com/rdk/services/vision" "go.viam.com/rdk/spatialmath" + "go.viam.com/rdk/utils" ) const ( @@ -576,16 +578,21 @@ func (ms *builtIn) newMoveOnGlobeRequest( // if we have some previous origin we've already recorded, // use that instead of resetting the origin - if ms.globeOrigin == nil { - ms.globeOrigin, _, err = movementSensor.Position(ctx, nil) + var globeOriginPoint *geo.Point + var globeHeading float64 + geoPoseOrigin, err := utils.AssertType[*spatialmath.GeoPose](req.Extra["globeOrigin"]) + if err != nil { + globeOriginPoint, _, err = movementSensor.Position(ctx, nil) if err != nil { return nil, err } - } - - heading, err := movementSensor.CompassHeading(ctx, nil) - if err != nil { - return nil, err + globeHeading, err = movementSensor.CompassHeading(ctx, nil) + if err != nil { + return nil, err + } + } else { + globeOriginPoint = geoPoseOrigin.Location() + globeHeading = geoPoseOrigin.Heading() } // add an offset between the movement sensor and the base if it is applicable @@ -596,7 +603,7 @@ func (ms *builtIn) newMoveOnGlobeRequest( movementSensorToBase = baseOrigin } // Create a localizer from the movement sensor, and collapse reported orientations to 2d - localizer := motion.TwoDLocalizer(motion.NewMovementSensorLocalizer(movementSensor, ms.globeOrigin, movementSensorToBase.Pose())) + localizer := motion.TwoDLocalizer(motion.NewMovementSensorLocalizer(movementSensor, globeOriginPoint, movementSensorToBase.Pose())) // create a KinematicBase from the componentName baseComponent, ok := ms.components[req.ComponentName] @@ -616,7 +623,7 @@ func (ms *builtIn) newMoveOnGlobeRequest( // Important: GeoPointToPose will create a pose such that incrementing latitude towards north increments +Y, and incrementing // longitude towards east increments +X. Heading is not taken into account. This pose must therefore be transformed based on the // orientation of the base such that it is a pose relative to the base's current location. - goalPoseRaw := spatialmath.NewPoseFromPoint(spatialmath.GeoPointToPoint(req.Destination, ms.globeOrigin)) + goalPoseRaw := spatialmath.NewPoseFromPoint(spatialmath.GeoPointToPoint(req.Destination, globeOriginPoint)) // construct limits straightlineDistance := goalPoseRaw.Point().Norm() if straightlineDistance > maxTravelDistanceMM { @@ -636,10 +643,10 @@ func (ms *builtIn) newMoveOnGlobeRequest( } // convert obstacles of type []GeoGeometry into []Geometry - geomsRaw := spatialmath.GeoGeometriesToGeometries(obstacles, ms.globeOrigin) + geomsRaw := spatialmath.GeoGeometriesToGeometries(obstacles, globeOriginPoint) // convert bounding regions which are GeoGeometries into Geometries - boundingRegions := spatialmath.GeoGeometriesToGeometries(req.BoundingRegions, ms.globeOrigin) + boundingRegions := spatialmath.GeoGeometriesToGeometries(req.BoundingRegions, globeOriginPoint) mr, err := ms.createBaseMoveRequest( ctx, @@ -657,7 +664,7 @@ func (ms *builtIn) newMoveOnGlobeRequest( mr.seedPlan = seedPlan mr.replanCostFactor = valExtra.replanCostFactor mr.requestType = requestTypeMoveOnGlobe - mr.geoPoseOrigin = spatialmath.NewGeoPose(ms.globeOrigin, heading) + mr.geoPoseOrigin = spatialmath.NewGeoPose(globeOriginPoint, globeHeading) mr.planRequest.BoundingRegions = boundingRegions return mr, nil } diff --git a/services/motion/builtin/state/state.go b/services/motion/builtin/state/state.go index 9091563d579..c9c19c5546c 100644 --- a/services/motion/builtin/state/state.go +++ b/services/motion/builtin/state/state.go @@ -12,6 +12,7 @@ import ( "github.com/google/uuid" "github.com/pkg/errors" + "go.viam.com/utils" "golang.org/x/exp/maps" @@ -20,6 +21,7 @@ import ( "go.viam.com/rdk/resource" "go.viam.com/rdk/services/motion" "go.viam.com/rdk/spatialmath" + rutils "go.viam.com/rdk/utils" ) // PlannerExecutor implements Plan and Execute. @@ -188,6 +190,18 @@ func (e *execution[R]) start(ctx context.Context) error { // replan default: replanCount++ + + // if we are dealing with a MoveOnGlobeReq, record the exsiting geoPose origin in the extras param + // so we can keep track of where the robot originally started + mogReq, err := rutils.AssertType[motion.MoveOnGlobeReq](e.req) + if err == nil { + mogReq.Extra["globeOrigin"] = originalPlanWithExecutor.executor.AnchorGeoPose() + e.req, err = rutils.AssertType[R](mogReq) + if err != nil { + e.notifyUnableToGiveGlobeOrigin(lastPWE.plan, err.Error(), time.Now()) + } + } + newPWE, err := e.newPlanWithExecutor(e.cancelCtx, lastPWE.plan.Plan, replanCount) // replan failed if err != nil { @@ -281,6 +295,17 @@ func (e *execution[R]) notifyStatePlanStopped(plan motion.PlanWithMetadata, time }) } +func (e *execution[R]) notifyUnableToGiveGlobeOrigin(plan motion.PlanWithMetadata, reason string, time time.Time) { + e.state.mu.Lock() + defer e.state.mu.Unlock() + e.state.updateStateStatusUpdate(stateUpdateMsg{ + componentName: e.componentName, + executionID: e.id, + planID: plan.ID, + planStatus: motion.PlanStatus{State: motion.PlanStateFailed, Timestamp: time, Reason: &reason}, + }) +} + // State is the state of the builtin motion service // It keeps track of the builtin motion service's executions. type State struct { From f9df2df41d87b33f5007147903aa425a2ecce8d6 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 2 Aug 2024 15:53:14 -0400 Subject: [PATCH 119/126] fix up inputtoprotobuf --- referenceframe/frame.go | 3 ++- services/motion/builtin/move_request.go | 35 +++++++++---------------- services/motion/builtin/state/state.go | 13 --------- services/motion/builtin/wrapperFrame.go | 5 ++-- 4 files changed, 17 insertions(+), 39 deletions(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 322e4a6d85b..f6fc102db79 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -542,9 +542,10 @@ func (pf *poseFrame) MarshalJSON() ([]byte, error) { // 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 { + 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 } diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index 6079babfdd9..f811d5891e3 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -9,7 +9,6 @@ import ( "sync" "time" - geo "github.com/kellydunn/golang-geo" "github.com/pkg/errors" goutils "go.viam.com/utils" @@ -27,7 +26,6 @@ import ( "go.viam.com/rdk/services/slam" "go.viam.com/rdk/services/vision" "go.viam.com/rdk/spatialmath" - "go.viam.com/rdk/utils" ) const ( @@ -576,23 +574,14 @@ func (ms *builtIn) newMoveOnGlobeRequest( return nil, resource.DependencyNotFoundError(req.MovementSensorName) } - // if we have some previous origin we've already recorded, - // use that instead of resetting the origin - var globeOriginPoint *geo.Point - var globeHeading float64 - geoPoseOrigin, err := utils.AssertType[*spatialmath.GeoPose](req.Extra["globeOrigin"]) + origin, _, err := movementSensor.Position(ctx, nil) if err != nil { - globeOriginPoint, _, err = movementSensor.Position(ctx, nil) - if err != nil { - return nil, err - } - globeHeading, err = movementSensor.CompassHeading(ctx, nil) - if err != nil { - return nil, err - } - } else { - globeOriginPoint = geoPoseOrigin.Location() - globeHeading = geoPoseOrigin.Heading() + return nil, err + } + + heading, err := movementSensor.CompassHeading(ctx, nil) + if err != nil { + return nil, err } // add an offset between the movement sensor and the base if it is applicable @@ -603,7 +592,7 @@ func (ms *builtIn) newMoveOnGlobeRequest( movementSensorToBase = baseOrigin } // Create a localizer from the movement sensor, and collapse reported orientations to 2d - localizer := motion.TwoDLocalizer(motion.NewMovementSensorLocalizer(movementSensor, globeOriginPoint, movementSensorToBase.Pose())) + localizer := motion.TwoDLocalizer(motion.NewMovementSensorLocalizer(movementSensor, origin, movementSensorToBase.Pose())) // create a KinematicBase from the componentName baseComponent, ok := ms.components[req.ComponentName] @@ -623,7 +612,7 @@ func (ms *builtIn) newMoveOnGlobeRequest( // Important: GeoPointToPose will create a pose such that incrementing latitude towards north increments +Y, and incrementing // longitude towards east increments +X. Heading is not taken into account. This pose must therefore be transformed based on the // orientation of the base such that it is a pose relative to the base's current location. - goalPoseRaw := spatialmath.NewPoseFromPoint(spatialmath.GeoPointToPoint(req.Destination, globeOriginPoint)) + goalPoseRaw := spatialmath.NewPoseFromPoint(spatialmath.GeoPointToPoint(req.Destination, origin)) // construct limits straightlineDistance := goalPoseRaw.Point().Norm() if straightlineDistance > maxTravelDistanceMM { @@ -643,10 +632,10 @@ func (ms *builtIn) newMoveOnGlobeRequest( } // convert obstacles of type []GeoGeometry into []Geometry - geomsRaw := spatialmath.GeoGeometriesToGeometries(obstacles, globeOriginPoint) + geomsRaw := spatialmath.GeoGeometriesToGeometries(obstacles, origin) // convert bounding regions which are GeoGeometries into Geometries - boundingRegions := spatialmath.GeoGeometriesToGeometries(req.BoundingRegions, globeOriginPoint) + boundingRegions := spatialmath.GeoGeometriesToGeometries(req.BoundingRegions, origin) mr, err := ms.createBaseMoveRequest( ctx, @@ -664,7 +653,7 @@ func (ms *builtIn) newMoveOnGlobeRequest( mr.seedPlan = seedPlan mr.replanCostFactor = valExtra.replanCostFactor mr.requestType = requestTypeMoveOnGlobe - mr.geoPoseOrigin = spatialmath.NewGeoPose(globeOriginPoint, globeHeading) + mr.geoPoseOrigin = spatialmath.NewGeoPose(origin, heading) mr.planRequest.BoundingRegions = boundingRegions return mr, nil } diff --git a/services/motion/builtin/state/state.go b/services/motion/builtin/state/state.go index c9c19c5546c..85247030c50 100644 --- a/services/motion/builtin/state/state.go +++ b/services/motion/builtin/state/state.go @@ -12,7 +12,6 @@ import ( "github.com/google/uuid" "github.com/pkg/errors" - "go.viam.com/utils" "golang.org/x/exp/maps" @@ -21,7 +20,6 @@ import ( "go.viam.com/rdk/resource" "go.viam.com/rdk/services/motion" "go.viam.com/rdk/spatialmath" - rutils "go.viam.com/rdk/utils" ) // PlannerExecutor implements Plan and Execute. @@ -191,17 +189,6 @@ func (e *execution[R]) start(ctx context.Context) error { default: replanCount++ - // if we are dealing with a MoveOnGlobeReq, record the exsiting geoPose origin in the extras param - // so we can keep track of where the robot originally started - mogReq, err := rutils.AssertType[motion.MoveOnGlobeReq](e.req) - if err == nil { - mogReq.Extra["globeOrigin"] = originalPlanWithExecutor.executor.AnchorGeoPose() - e.req, err = rutils.AssertType[R](mogReq) - if err != nil { - e.notifyUnableToGiveGlobeOrigin(lastPWE.plan, err.Error(), time.Now()) - } - } - newPWE, err := e.newPlanWithExecutor(e.cancelCtx, lastPWE.plan.Plan, replanCount) // replan failed if err != nil { diff --git a/services/motion/builtin/wrapperFrame.go b/services/motion/builtin/wrapperFrame.go index c339e8c738b..a3f85ac2196 100644 --- a/services/motion/builtin/wrapperFrame.go +++ b/services/motion/builtin/wrapperFrame.go @@ -137,9 +137,10 @@ func (wf *wrapperFrame) InputFromProtobuf(jp *pb.JointPositions) []referencefram // ProtobufFromInput converts inputs to pb.JointPosition. func (wf *wrapperFrame) ProtobufFromInput(input []referenceframe.Input) *pb.JointPositions { n := make([]float64, len(input)) - for idx, a := range input { - n[idx] = utils.RadToDeg(a.Value) + for idx, a := range input[:len(input)-1] { + n[idx] = a.Value } + n[len(input)-1] = utils.DegToRad(input[len(input)-1].Value) return &pb.JointPositions{Values: n} } From 41c69cc867c1172d7a8d87d42af963c1d559fada Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 2 Aug 2024 16:02:20 -0400 Subject: [PATCH 120/126] remove notify function from state --- services/motion/builtin/state/state.go | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/services/motion/builtin/state/state.go b/services/motion/builtin/state/state.go index 85247030c50..86b7e27d425 100644 --- a/services/motion/builtin/state/state.go +++ b/services/motion/builtin/state/state.go @@ -282,17 +282,6 @@ func (e *execution[R]) notifyStatePlanStopped(plan motion.PlanWithMetadata, time }) } -func (e *execution[R]) notifyUnableToGiveGlobeOrigin(plan motion.PlanWithMetadata, reason string, time time.Time) { - e.state.mu.Lock() - defer e.state.mu.Unlock() - e.state.updateStateStatusUpdate(stateUpdateMsg{ - componentName: e.componentName, - executionID: e.id, - planID: plan.ID, - planStatus: motion.PlanStatus{State: motion.PlanStateFailed, Timestamp: time, Reason: &reason}, - }) -} - // State is the state of the builtin motion service // It keeps track of the builtin motion service's executions. type State struct { From c80d029f4ca67f59fd6e72fa7258d6d6f83a6eb9 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Fri, 2 Aug 2024 16:34:12 -0400 Subject: [PATCH 121/126] remove empty space in state.go --- services/motion/builtin/state/state.go | 1 - 1 file changed, 1 deletion(-) diff --git a/services/motion/builtin/state/state.go b/services/motion/builtin/state/state.go index 86b7e27d425..9091563d579 100644 --- a/services/motion/builtin/state/state.go +++ b/services/motion/builtin/state/state.go @@ -188,7 +188,6 @@ func (e *execution[R]) start(ctx context.Context) error { // replan default: replanCount++ - newPWE, err := e.newPlanWithExecutor(e.cancelCtx, lastPWE.plan.Plan, replanCount) // replan failed if err != nil { From 7b0460c960a9128bf7eddb2b76875e4db813e1d1 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 5 Aug 2024 12:45:44 -0400 Subject: [PATCH 122/126] change how limits are specified for the poseFrame --- referenceframe/frame.go | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index f6fc102db79..baffdbf3a20 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -462,13 +462,13 @@ func NewPoseFrame(name string, geometry []spatial.Geometry) (Frame, error) { } orientationVector.Normalize() 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: -orientationVector.OX, Max: orientationVector.OX}, // OX - {Min: -orientationVector.OY, Max: orientationVector.OY}, // OY - {Min: -orientationVector.OZ, Max: orientationVector.OZ}, // OZ - {Min: -orientationVector.Theta, Max: orientationVector.Theta}, // Theta + {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}, From 60ff57b69279037370f28e9bf60e95518737bd75 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 5 Aug 2024 13:02:07 -0400 Subject: [PATCH 123/126] update InputFromProtobuf and ProtobufFromInput for poseFramen and wrapperFrame --- referenceframe/frame.go | 4 ++-- services/motion/builtin/wrapperFrame.go | 17 ++++++++++------- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index baffdbf3a20..2bf285e8434 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -545,7 +545,7 @@ func (pf *poseFrame) InputFromProtobuf(jp *pb.JointPositions) []Input { 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])} + n[len(jp.Values)-1] = Input{utils.RadToDeg(jp.Values[len(jp.Values)-1])} return n } @@ -553,7 +553,7 @@ func (pf *poseFrame) InputFromProtobuf(jp *pb.JointPositions) []Input { func (pf *poseFrame) ProtobufFromInput(input []Input) *pb.JointPositions { n := make([]float64, len(input)) for idx, a := range input { - n[idx] = utils.RadToDeg(a.Value) + n[idx] = a.Value } return &pb.JointPositions{Values: n} } diff --git a/services/motion/builtin/wrapperFrame.go b/services/motion/builtin/wrapperFrame.go index a3f85ac2196..6005b03f564 100644 --- a/services/motion/builtin/wrapperFrame.go +++ b/services/motion/builtin/wrapperFrame.go @@ -127,20 +127,23 @@ func (wf *wrapperFrame) MarshalJSON() ([]byte, error) { // InputFromProtobuf converts pb.JointPosition to inputs. func (wf *wrapperFrame) InputFromProtobuf(jp *pb.JointPositions) []referenceframe.Input { - n := make([]referenceframe.Input, len(jp.Values)) - for idx, d := range jp.Values { - n[idx] = referenceframe.Input{Value: d} - } - return n + jpValues := jp.GetValues() + + 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 { n := make([]float64, len(input)) - for idx, a := range input[:len(input)-1] { + for idx, a := range input { n[idx] = a.Value } - n[len(input)-1] = utils.DegToRad(input[len(input)-1].Value) return &pb.JointPositions{Values: n} } From 9c778f4942d55a975286bd5b3c054bc4095c833f Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 5 Aug 2024 13:11:10 -0400 Subject: [PATCH 124/126] update InputFromProtobuf and ProtobufFromInput for poseFramen and wrapperFrame v2 --- referenceframe/frame.go | 3 +-- services/motion/builtin/wrapperFrame.go | 12 +++++++----- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index 2bf285e8434..c6f2f274b94 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -542,10 +542,9 @@ func (pf *poseFrame) MarshalJSON() ([]byte, error) { // 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] { + for idx, d := range jp.Values { n[idx] = Input{d} } - n[len(jp.Values)-1] = Input{utils.RadToDeg(jp.Values[len(jp.Values)-1])} return n } diff --git a/services/motion/builtin/wrapperFrame.go b/services/motion/builtin/wrapperFrame.go index 6005b03f564..797173b7eb0 100644 --- a/services/motion/builtin/wrapperFrame.go +++ b/services/motion/builtin/wrapperFrame.go @@ -140,11 +140,13 @@ func (wf *wrapperFrame) InputFromProtobuf(jp *pb.JointPositions) []referencefram // ProtobufFromInput converts inputs to pb.JointPosition. func (wf *wrapperFrame) ProtobufFromInput(input []referenceframe.Input) *pb.JointPositions { - n := make([]float64, len(input)) - for idx, a := range input { - n[idx] = a.Value - } - return &pb.JointPositions{Values: n} + 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.GetValues(), localizationFrameJP.GetValues()...)} } func (wf *wrapperFrame) PTGSolvers() []tpspace.PTGSolver { From e0099c991714a0352d86301cb367d22a7467e646 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 5 Aug 2024 13:15:18 -0400 Subject: [PATCH 125/126] update InputFromProtobuf and ProtobufFromInput for poseFramen and wrapperFrame v3 --- referenceframe/frame.go | 6 ++++-- services/motion/builtin/wrapperFrame.go | 4 ++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index c6f2f274b94..c25b9301de9 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -542,18 +542,20 @@ func (pf *poseFrame) MarshalJSON() ([]byte, error) { // 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 { + 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 { + 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} } diff --git a/services/motion/builtin/wrapperFrame.go b/services/motion/builtin/wrapperFrame.go index 797173b7eb0..5f1eb3a2a49 100644 --- a/services/motion/builtin/wrapperFrame.go +++ b/services/motion/builtin/wrapperFrame.go @@ -127,7 +127,7 @@ func (wf *wrapperFrame) MarshalJSON() ([]byte, error) { // InputFromProtobuf converts pb.JointPosition to inputs. func (wf *wrapperFrame) InputFromProtobuf(jp *pb.JointPositions) []referenceframe.Input { - jpValues := jp.GetValues() + jpValues := jp.Values executionFrameSubset := jpValues[:len(wf.executionFrame.DoF())] executionFrameInputs := wf.executionFrame.InputFromProtobuf(&pb.JointPositions{Values: executionFrameSubset}) @@ -146,7 +146,7 @@ func (wf *wrapperFrame) ProtobufFromInput(input []referenceframe.Input) *pb.Join localizationFrameSubset := input[len(wf.executionFrame.DoF()):] localizationFrameJP := wf.localizationFrame.ProtobufFromInput(localizationFrameSubset) - return &pb.JointPositions{Values: append(executionFrameJP.GetValues(), localizationFrameJP.GetValues()...)} + return &pb.JointPositions{Values: append(executionFrameJP.Values, localizationFrameJP.Values...)} } func (wf *wrapperFrame) PTGSolvers() []tpspace.PTGSolver { From a6b4d5f77ad15780abea1f0f8e11ab7df2e0dac7 Mon Sep 17 00:00:00 2001 From: Nick Franczak Date: Mon, 5 Aug 2024 14:21:59 -0400 Subject: [PATCH 126/126] remove rsdk-8340 todo + remove old specification of limits for the poseFrame initialization --- referenceframe/frame.go | 7 ------- services/motion/builtin/move_request.go | 1 - 2 files changed, 8 deletions(-) diff --git a/referenceframe/frame.go b/referenceframe/frame.go index c25b9301de9..4bbf36726e7 100644 --- a/referenceframe/frame.go +++ b/referenceframe/frame.go @@ -454,13 +454,6 @@ type poseFrame struct { // 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) { - orientationVector := spatial.OrientationVector{ - OX: math.Inf(1), - OY: math.Inf(1), - OZ: math.Inf(1), - Theta: math.Inf(1), - } - orientationVector.Normalize() limits := []Limit{ {Min: math.Inf(-1), Max: math.Inf(1)}, // X {Min: math.Inf(-1), Max: math.Inf(1)}, // Y diff --git a/services/motion/builtin/move_request.go b/services/motion/builtin/move_request.go index f811d5891e3..a1193cc3410 100644 --- a/services/motion/builtin/move_request.go +++ b/services/motion/builtin/move_request.go @@ -183,7 +183,6 @@ func (mr *moveRequest) deviatedFromPlan(ctx context.Context, plan motionplan.Pla // getTransientDetections returns a list of geometries as observed by the provided vision service and camera. // Depending on the caller, the geometries returned are either in their relative position // with respect to the base or in their absolute position with respect to the world. -// TODO(RSDK-8340): Explore usefulness of pointcloud instead of spatialmath.Geometry. func (mr *moveRequest) getTransientDetections( ctx context.Context, visSrvc vision.Service,