motionplan

package
v0.47.0 Latest Latest
Warning

This package is not in the latest version of its module.

 Go to latest Published: Oct 21, 2024 License: AGPL-3.0 Imports: 27 Imported by: 1

Documentation

Overview

Package motionplan is a motion planning library.

Package motionplan is a motion planning library.

Index

Constants

View Source 
const (
	FreeMotionProfile         = "free"
	LinearMotionProfile       = "linear"
	PseudolinearMotionProfile = "pseudolinear"
	OrientationMotionProfile  = "orientation"
	PositionOnlyMotionProfile = "position_only"
)

TODO: Make this an enum the set of supported motion profiles.

Variables

This section is empty.

Functions

func CalculateFrameErrorState added in v0.33.0 

func CalculateFrameErrorState(e ExecutionState, executionFrame, localizationFrame referenceframe.Frame) (spatialmath.Pose, error)

CalculateFrameErrorState takes an ExecutionState and a Frame and calculates the error between the Frame's expected and actual positions.

func CheckPlan added in v0.9.0 

func CheckPlan(
	checkFrame referenceframe.Frame,
	executionState ExecutionState,
	worldState *referenceframe.WorldState,
	fs referenceframe.FrameSystem,
	lookAheadDistanceMM float64,
	logger logging.Logger,
) error

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 PathStepCount added in v0.2.4 

func PathStepCount(seedPos, goalPos spatialmath.Pose, stepSize float64) int

PathStepCount will determine the number of steps which should be used to get from the seed to the goal. The returned value is guaranteed to be at least 1. stepSize represents both the max mm movement per step, and max R4AA degrees per step.

func PlanFrameMotion added in v0.2.2 

func PlanFrameMotion(ctx context.Context,
	logger logging.Logger,
	dst spatialmath.Pose,
	f frame.Frame,
	seed []frame.Input,
	constraints *Constraints,
	planningOpts map[string]interface{},
) ([][]frame.Input, error)

PlanFrameMotion plans a motion to destination for a given frame with no frame system. It will create a new FS just for the plan. WorldState is not supported in the absence of a real frame system.

Types

type Collision 

type Collision struct {
	// contains filtered or unexported fields
}

Collision is a pair of strings corresponding to names of Geometry objects in collision, and a penetrationDepth describing the Euclidean distance a Geometry would have to be moved to resolve the Collision.

type CollisionSpecification added in v0.29.0 

type CollisionSpecification struct {
	// Pairs of frame which should be allowed to collide with one another
	Allows []CollisionSpecificationAllowedFrameCollisions
}

CollisionSpecification is used to selectively apply obstacle avoidance to specific parts of the robot.

type CollisionSpecificationAllowedFrameCollisions added in v0.29.0 

type CollisionSpecificationAllowedFrameCollisions struct {
	Frame1, Frame2 string
}

CollisionSpecificationAllowedFrameCollisions is used to define frames that are allowed to collide.

type ConstraintHandler added in v0.2.34 

type ConstraintHandler struct {
	// contains filtered or unexported fields
}

ConstraintHandler is a convenient wrapper for constraint handling which is likely to be common among most motion planners. Including a constraint handler as an anonymous struct member allows reuse.

func (*ConstraintHandler) AddSegmentConstraint added in v0.2.34 

func (c *ConstraintHandler) AddSegmentConstraint(name string, cons SegmentConstraint)

AddSegmentConstraint will add or overwrite a constraint function with a given name. A constraint function should return true if the given position satisfies the constraint.

func (*ConstraintHandler) AddStateConstraint added in v0.2.34 

func (c *ConstraintHandler) AddStateConstraint(name string, cons StateConstraint)

AddStateConstraint will add or overwrite a constraint function with a given name. A constraint function should return true if the given position satisfies the constraint.

func (*ConstraintHandler) CheckSegmentAndStateValidity added in v0.2.34 

func (c *ConstraintHandler) CheckSegmentAndStateValidity(segment *ik.Segment, resolution float64) (bool, *ik.Segment)

CheckSegmentAndStateValidity will check an segment input and confirm that it 1) meets all segment constraints, and 2) meets all state constraints across the segment at some resolution. If it fails an intermediate state, it will return the shortest valid segment, provided that segment also meets segment constraints.

func (*ConstraintHandler) CheckSegmentConstraints added in v0.2.34 

func (c *ConstraintHandler) CheckSegmentConstraints(segment *ik.Segment) (bool, string)

CheckSegmentConstraints will check a given input against all segment constraints. Return values are: -- a bool representing whether all constraints passed -- if failing, a string naming the failed constraint.

func (*ConstraintHandler) CheckStateConstraints added in v0.2.34 

func (c *ConstraintHandler) CheckStateConstraints(state *ik.State) (bool, string)

CheckStateConstraints will check a given input against all state constraints. Return values are: -- a bool representing whether all constraints passed -- if failing, a string naming the failed constraint.

func (*ConstraintHandler) CheckStateConstraintsAcrossSegment added in v0.2.34 

func (c *ConstraintHandler) CheckStateConstraintsAcrossSegment(ci *ik.Segment, resolution float64) (bool, *ik.Segment)

CheckStateConstraintsAcrossSegment will interpolate the given input from the StartInput to the EndInput, and ensure that all intermediate states as well as both endpoints satisfy all state constraints. If all constraints are satisfied, then this will return `true, nil`. If any constraints fail, this will return false, and an Segment representing the valid portion of the segment, if any. If no part of the segment is valid, then `false, nil` is returned.

func (*ConstraintHandler) RemoveSegmentConstraint added in v0.2.34 

func (c *ConstraintHandler) RemoveSegmentConstraint(name string)

RemoveSegmentConstraint will remove the given constraint.

func (*ConstraintHandler) RemoveStateConstraint added in v0.2.34 

func (c *ConstraintHandler) RemoveStateConstraint(name string)

RemoveStateConstraint will remove the given constraint.

func (*ConstraintHandler) SegmentConstraints added in v0.2.34 

func (c *ConstraintHandler) SegmentConstraints() []string

SegmentConstraints will list all segment constraints by name.

func (*ConstraintHandler) StateConstraints added in v0.2.34 

func (c *ConstraintHandler) StateConstraints() []string

StateConstraints will list all state constraints by name.

type Constraints added in v0.29.0 

type Constraints struct {
	LinearConstraint       []LinearConstraint
	OrientationConstraint  []OrientationConstraint
	CollisionSpecification []CollisionSpecification
}

Constraints is a struct to store the constraints imposed upon a robot It serves as a convenenient RDK wrapper for the protobuf object.

func ConstraintsFromProtobuf added in v0.29.0 

func ConstraintsFromProtobuf(pbConstraint *motionpb.Constraints) *Constraints

ConstraintsFromProtobuf converts a protobuf object to a Constraints object.

func NewConstraints added in v0.29.0 

func NewConstraints(
	linConstraints []LinearConstraint,
	orientConstraints []OrientationConstraint,
	collSpecifications []CollisionSpecification,
) *Constraints

NewConstraints initializes a Constraints object with user-defined LinearConstraint, OrientationConstraint, and CollisionSpecification.

func NewEmptyConstraints added in v0.29.0 

func NewEmptyConstraints() *Constraints

NewEmptyConstraints creates a new, empty Constraints object.

func (*Constraints) AddCollisionSpecification added in v0.29.0 

func (c *Constraints) AddCollisionSpecification(collConstraint CollisionSpecification)

AddCollisionSpecification appends a CollisionSpecification to a Constraints object.

func (*Constraints) AddLinearConstraint added in v0.29.0 

func (c *Constraints) AddLinearConstraint(linConstraint LinearConstraint)

AddLinearConstraint appends a LinearConstraint to a Constraints object.

func (*Constraints) AddOrientationConstraint added in v0.29.0 

func (c *Constraints) AddOrientationConstraint(orientConstraint OrientationConstraint)

AddOrientationConstraint appends a OrientationConstraint to a Constraints object.

func (*Constraints) GetCollisionSpecification added in v0.30.0 

func (c *Constraints) GetCollisionSpecification() []CollisionSpecification

GetCollisionSpecification checks if the Constraints object is nil and if not then returns its CollisionSpecification field.

func (*Constraints) GetLinearConstraint added in v0.30.0 

func (c *Constraints) GetLinearConstraint() []LinearConstraint

GetLinearConstraint checks if the Constraints object is nil and if not then returns its LinearConstraint field.

func (*Constraints) GetOrientationConstraint added in v0.30.0 

func (c *Constraints) GetOrientationConstraint() []OrientationConstraint

GetOrientationConstraint checks if the Constraints object is nil and if not then returns its OrientationConstraint field.

func (*Constraints) ToProtobuf added in v0.29.0 

func (c *Constraints) ToProtobuf() *motionpb.Constraints

ToProtobuf takes an existing Constraints object and converts it to a protobuf.

type ExecutionState added in v0.27.0 

type ExecutionState struct {
	// contains filtered or unexported fields
}

ExecutionState describes a plan and a particular state along it.

func NewExecutionState added in v0.27.0 

func NewExecutionState(
	plan Plan,
	index int,
	currentInputs map[string][]referenceframe.Input,
	currentPose map[string]*referenceframe.PoseInFrame,
) (ExecutionState, error)

NewExecutionState will construct an ExecutionState struct.

func (*ExecutionState) CurrentInputs added in v0.27.0 

func (e *ExecutionState) CurrentInputs() map[string][]referenceframe.Input

CurrentInputs returns the current inputs of the components associated with the ExecutionState.

func (*ExecutionState) CurrentPoses added in v0.27.0 

func (e *ExecutionState) CurrentPoses() map[string]*referenceframe.PoseInFrame

CurrentPoses returns the current poses in frame of the components associated with the ExecutionState.

func (*ExecutionState) Index added in v0.27.0 

func (e *ExecutionState) Index() int

Index returns the currently-executing index of the execution state's Plan.

func (*ExecutionState) Plan added in v0.27.0 

func (e *ExecutionState) Plan() Plan

Plan returns the plan associated with the execution state.

type LinearConstraint added in v0.29.0 

type LinearConstraint struct {
	LineToleranceMm          float64 // Max linear deviation from straight-line between start and goal, in mm.
	OrientationToleranceDegs float64
}

LinearConstraint specifies that the component being moved should move linearly relative to its goal. It does not constrain the motion of components other than the `component_name` specified in motion.Move.

type OrientationConstraint added in v0.29.0 

type OrientationConstraint struct {
	OrientationToleranceDegs float64
}

OrientationConstraint specifies that the component being moved will not deviate its orientation beyond some threshold relative to the goal. It does not constrain the motion of components other than the `component_name` specified in motion.Move.

type Path added in v0.21.0 

type Path []PathStep

Path is a slice of PathSteps describing a series of Poses for a robot to travel to in the course of following a Plan. The pose of the PathStep is the pose at the end of the corresponding set of inputs in the Trajectory.

func (Path) GetFramePoses added in v0.21.0 

func (path Path) GetFramePoses(frameName string) ([]spatialmath.Pose, error)

GetFramePoses returns a slice of poses a given frame should visit in the course of the Path.

func (Path) String added in v0.21.0 

func (path Path) String() string

type PathStep added in v0.21.0 

type PathStep map[string]*referenceframe.PoseInFrame

PathStep is a mapping of Frame names to PoseInFrames.

func PathStepFromProto added in v0.21.0 

func PathStepFromProto(ps *pb.PlanStep) (PathStep, error)

PathStepFromProto converts a *pb.PlanStep to a PlanStep.

func (PathStep) ToProto added in v0.21.0 

func (ps PathStep) ToProto() *pb.PlanStep

ToProto converts a PathStep to its representation in protobuf.

type Plan added in v0.9.0 

type Plan interface {
	Trajectory() Trajectory
	Path() Path
}

Plan is an interface that describes plans returned by this package. There are two key components to a Plan: Its Trajectory contains information pertaining to the commands required to actuate the robot to realize the Plan. Its Path contains information describing the Pose of the robot as it travels the Plan.

func NewGeoPlan added in v0.21.0 

func NewGeoPlan(plan Plan, pt *geo.Point) Plan

NewGeoPlan returns a Plan containing a Path with GPS coordinates smuggled into the Pose struct. Each GPS point is created using: A Point with X as the longitude and Y as the latitude An orientation using the heading as the theta in an OrientationVector with Z=1.

func OffsetPlan added in v0.21.0 

func OffsetPlan(plan Plan, offset spatialmath.Pose) Plan

OffsetPlan returns a new Plan that is equivalent to the given Plan if its Path was offset by the given Pose. Does not modify Trajectory.

func PlanMotion added in v0.0.8 

func PlanMotion(ctx context.Context, request *PlanRequest) (Plan, error)

PlanMotion plans a motion from a provided plan request.

func RemainingPlan added in v0.21.0 

func RemainingPlan(plan Plan, waypointIndex int) (Plan, error)

RemainingPlan returns a new Plan equal to the given plan from the waypointIndex onwards.

func Replan added in v0.12.0 

func Replan(ctx context.Context, request *PlanRequest, currentPlan Plan, replanCostFactor float64) (Plan, error)

Replan plans a motion from a provided plan request, and then will return that plan only if its cost is better than the cost of the passed-in plan multiplied by `replanCostFactor`.

type PlanRequest added in v0.9.0 

type PlanRequest struct {
	Logger             logging.Logger
	Goal               *frame.PoseInFrame
	Frame              frame.Frame
	FrameSystem        frame.FrameSystem
	StartPose          spatialmath.Pose
	StartConfiguration map[string][]frame.Input
	WorldState         *frame.WorldState
	BoundingRegions    []spatialmath.Geometry
	Constraints        *Constraints
	Options            map[string]interface{}
}

PlanRequest is a struct to store all the data necessary to make a call to PlanMotion.

type SegmentConstraint added in v0.2.34 

type SegmentConstraint func(*ik.Segment) bool

SegmentConstraint tests whether a transition from a starting robot configuration to an ending robot configuration is valid. If the returned bool is true, the constraint is satisfied and the segment is valid.

type SimplePlan added in v0.21.0 

type SimplePlan struct {
	// contains filtered or unexported fields
}

SimplePlan is a struct containing a Path and a Trajectory, together these comprise a Plan.

func NewSimplePlan added in v0.21.0 

func NewSimplePlan(path Path, traj Trajectory) *SimplePlan

NewSimplePlan instantiates a new Plan from a Path and Trajectory.

func (*SimplePlan) Path added in v0.21.0 

func (plan *SimplePlan) Path() Path

Path returns the Path associated with the Plan.

func (*SimplePlan) Trajectory added in v0.21.0 

func (plan *SimplePlan) Trajectory() Trajectory

Trajectory returns the Trajectory associated with the Plan.

type StateConstraint added in v0.2.34 

type StateConstraint func(*ik.State) bool

StateConstraint tests whether a given robot configuration is valid If the returned bool is true, the constraint is satisfied and the state is valid.

func NewAbsoluteLinearInterpolatingConstraint added in v0.0.8 

func NewAbsoluteLinearInterpolatingConstraint(from, to spatial.Pose, linTol, orientTol float64) (StateConstraint, ik.StateMetric)

NewAbsoluteLinearInterpolatingConstraint provides a Constraint whose valid manifold allows a specified amount of deviation from the shortest straight-line path between the start and the goal. linTol is the allowed linear deviation in mm, orientTol is the allowed orientation deviation measured by norm of the R3AA orientation difference to the slerp path between start/goal orientations.

func NewBoundingRegionConstraint added in v0.29.0 

func NewBoundingRegionConstraint(robotGeoms, boundingRegions []spatial.Geometry, collisionBufferMM float64) StateConstraint

NewBoundingRegionConstraint will determine if the given list of robot geometries are in collision with the given list of bounding regions.

func NewCollisionConstraint 

func NewCollisionConstraint(
	moving, static []spatial.Geometry,
	collisionSpecifications []*Collision,
	reportDistances bool,
	collisionBufferMM float64,
) (StateConstraint, error)

NewCollisionConstraint is the most general method to create a collision constraint, which will be violated if geometries constituting the given frame ever come into collision with obstacle geometries outside of the collisions present for the observationInput. Collisions specified as collisionSpecifications will also be ignored if reportDistances is false, this check will be done as fast as possible, if true maximum information will be available for debugging.

func NewLineConstraint 

func NewLineConstraint(pt1, pt2 r3.Vector, tolerance float64) (StateConstraint, ik.StateMetric)

NewLineConstraint is used to define a constraint space for a line, and will return 1) a constraint function which will determine whether a point is on the line, and 2) a distance function which will bring a pose into the valid constraint space. tolerance refers to the closeness to the line necessary to be a valid pose in mm.

func NewOctreeCollisionConstraint added in v0.2.34 

func NewOctreeCollisionConstraint(octree *pointcloud.BasicOctree, threshold int, buffer, collisionBufferMM float64) StateConstraint

NewOctreeCollisionConstraint takes an octree and will return a constraint that checks whether any of the geometries in the solver frame intersect with points in the octree. Threshold sets the confidence level required for a point to be considered, and buffer is the distance to a point that is considered a collision in mm.

func NewPlaneConstraint 

func NewPlaneConstraint(pNorm, pt r3.Vector, writingAngle, epsilon float64) (StateConstraint, ik.StateMetric)

NewPlaneConstraint is used to define a constraint space for a plane, and will return 1) a constraint function which will determine whether a point is on the plane and in a valid orientation, and 2) a distance function which will bring a pose into the valid constraint space. The plane normal is assumed to point towards the valid area. angle refers to the maximum unit sphere segment length deviation from the ov epsilon refers to the closeness to the plane necessary to be a valid pose.

func NewProportionalLinearInterpolatingConstraint added in v0.0.8 

func NewProportionalLinearInterpolatingConstraint(from, to spatial.Pose, epsilon float64) (StateConstraint, ik.StateMetric)

NewProportionalLinearInterpolatingConstraint will provide the same metric and constraint as NewAbsoluteLinearInterpolatingConstraint, except that allowable linear and orientation deviation is scaled based on the distance from start to goal.

func NewSlerpOrientationConstraint 

func NewSlerpOrientationConstraint(start, goal spatial.Pose, tolerance float64) (StateConstraint, ik.StateMetric)

NewSlerpOrientationConstraint will measure the orientation difference between the orientation of two poses, and return a constraint that returns whether a given orientation is within a given tolerance distance of the shortest segment between the two orientations, as well as a metric which returns the distance to that valid region.

type Trajectory added in v0.21.0 

type Trajectory []map[string][]referenceframe.Input

Trajectory is a slice of maps describing a series of Inputs for a robot to travel to in the course of following a Plan. Each item in this slice maps a Frame's name (found by calling frame.Name()) to the Inputs that Frame should be modified by.

func (Trajectory) EvaluateCost added in v0.21.0 

func (traj Trajectory) EvaluateCost(distFunc ik.SegmentMetric) float64

EvaluateCost calculates a cost to a trajectory as measured by the given distFunc Metric.

func (Trajectory) GetFrameInputs added in v0.21.0 

func (traj Trajectory) GetFrameInputs(frameName string) ([][]referenceframe.Input, error)

GetFrameInputs is a helper function which will extract the waypoints of a single frame from the map output of a trajectory.

func (Trajectory) String added in v0.21.0 

func (traj Trajectory) String() string

String returns a human-readable version of the trajectory, suitable for debugging.

Source Files

View all Source files

Directories

Path Synopsis
Package ik contains tols for doing gradient-descent based inverse kinematics, allowing for the minimization of arbitrary metrics based on the output of calling `Transform` on the given frame.
 Package ik contains tols for doing gradient-descent based inverse kinematics, allowing for the minimization of arbitrary metrics based on the output of calling `Transform` on the given frame.
Package tpspace defines an assortment of precomputable trajectories which can be used to plan nonholonomic 2d motion
 Package tpspace defines an assortment of precomputable trajectories which can be used to plan nonholonomic 2d motion

Jump to

Keyboard shortcuts

? : This menu
/ : Search site
f or F : Jump to
y or Y : Canonical URL
go.dev uses cookies from Google to deliver and enhance the quality of its services and to analyze traffic. Learn more.