motionplan

package
v0.23.0 Latest Latest
Warning

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

 Go to latest Published: Mar 15, 2024 License: AGPL-3.0 Imports: 28 Imported by: 0

Documentation

Overview

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 CheckPlan added in v0.9.0 

func CheckPlan(
	checkFrame frame.Frame,
	plan Plan,
	worldState *frame.WorldState,
	fs frame.FrameSystem,
	currentPose spatialmath.Pose,
	currentInputs map[string][]frame.Input,
	errorState spatialmath.Pose,
	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 ComputeOOBPosition added in v0.2.15 

func ComputeOOBPosition(model referenceframe.Frame, joints *pb.JointPositions) (spatialmath.Pose, error)

ComputeOOBPosition takes a model and a protobuf JointPositions in degrees and returns the cartesian position of the end effector as a protobuf ArmPosition even when the arm is in an out of bounds state. This is performed statelessly without changing any data.

func ComputePosition 

func ComputePosition(model referenceframe.Frame, joints *pb.JointPositions) (spatialmath.Pose, error)

ComputePosition takes a model and a protobuf JointPositions in degrees and returns the cartesian position of the end effector as a protobuf ArmPosition. This is performed statelessly without changing any data.

func L2Distance 

func L2Distance(q1, q2 []float64) float64

L2Distance returns the L2 normalized difference between two equal length arrays.

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,
	constraintSpec *pb.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 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 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.

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
	StartConfiguration map[string][]frame.Input
	WorldState         *frame.WorldState
	ConstraintSpecs    *pb.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 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.