motionplan

package
v0.7.3-rc0 Latest Latest
Warning

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

 Go to latest Published: Aug 17, 2023 License: AGPL-3.0 Imports: 27 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"
)

the set of supported motion profiles.

Variables

This section is empty.

Functions

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 EvaluatePlan added in v0.1.0 

func EvaluatePlan(plan [][]referenceframe.Input, distFunc SegmentMetric) (totalCost float64)

EvaluatePlan assigns a numeric score to a plan that corresponds to the cumulative distance between input waypoints in the plan.

func FrameStepsFromRobotPath added in v0.2.2 

func FrameStepsFromRobotPath(frameName string, path []map[string][]referenceframe.Input) ([][]referenceframe.Input, error)

FrameStepsFromRobotPath is a helper function which will extract the waypoints of a single frame from the map output of a robot path.

func JointMetric added in v0.2.34 

func JointMetric(segment *Segment) float64

JointMetric is a metric which will sum the squared differences in each input from start to end.

func L2Distance 

func L2Distance(q1, q2 []float64) float64

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

func L2InputMetric added in v0.2.34 

func L2InputMetric(segment *Segment) float64

L2InputMetric is a metric which will return a L2 norm of the StartConfiguration and EndConfiguration in an arc input.

func NewAbsoluteLinearInterpolatingConstraint added in v0.0.8 

func NewAbsoluteLinearInterpolatingConstraint(from, to spatial.Pose, linTol, orientTol float64) (StateConstraint, 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 NewLineConstraint 

func NewLineConstraint(pt1, pt2 r3.Vector, tolerance float64) (StateConstraint, 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 NewPlaneConstraint 

func NewPlaneConstraint(pNorm, pt r3.Vector, writingAngle, epsilon float64) (StateConstraint, 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, 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, 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.

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 golog.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.

func PlanMotion added in v0.0.8 

func PlanMotion(ctx context.Context,
	logger golog.Logger,
	dst *frame.PoseInFrame,
	f frame.Frame,
	seedMap map[string][]frame.Input,
	fs frame.FrameSystem,
	worldState *frame.WorldState,
	constraintSpec *pb.Constraints,
	planningOpts map[string]interface{},
) ([]map[string][]frame.Input, error)

PlanMotion plans a motion to destination for a given frame. It takes a given frame system, wraps it with a SolvableFS, and solves.

func SquaredNormNoOrientSegmentMetric added in v0.3.0 

func SquaredNormNoOrientSegmentMetric(segment *Segment) float64

SquaredNormNoOrientSegmentMetric is a metric which will return the cartesian distance between the two positions.

func SquaredNormSegmentMetric added in v0.3.0 

func SquaredNormSegmentMetric(segment *Segment) float64

SquaredNormSegmentMetric is a metric which will return the cartesian distance between the two positions.

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 CombinedIK 

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

CombinedIK defines the fields necessary to run a combined solver.

func CreateCombinedIKSolver 

func CreateCombinedIKSolver(model referenceframe.Frame, logger golog.Logger, nCPU int, goalThreshold float64) (*CombinedIK, error)

CreateCombinedIKSolver creates a combined parallel IK solver with a number of nlopt solvers equal to the nCPU passed in. Each will be given a different random seed. When asked to solve, all solvers will be run in parallel and the first valid found solution will be returned.

func (*CombinedIK) Frame 

func (ik *CombinedIK) Frame() referenceframe.Frame

Frame returns the associated referenceframe.

func (*CombinedIK) Solve 

func (ik *CombinedIK) Solve(ctx context.Context,
	c chan<- []referenceframe.Input,
	seed []referenceframe.Input,
	m StateMetric,
	rseed int,
) error

Solve will initiate solving for the given position in all child solvers, seeding with the specified initial joint positions. If unable to solve, the returned error will be non-nil.

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 *Segment, resolution float64) (bool, *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 *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 *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 *Segment, resolution float64) (bool, *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 InverseKinematics 

type InverseKinematics interface {
	// Solve receives a context, the goal arm position, and current joint angles.
	Solve(context.Context, chan<- []referenceframe.Input, []referenceframe.Input, StateMetric, int) error
}

InverseKinematics defines an interface which, provided with seed inputs and a Metric to minimize to zero, will output all found solutions to the provided channel until cancelled or otherwise completes.

type NloptIK 

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

NloptIK TODO.

func CreateNloptIKSolver 

func CreateNloptIKSolver(mdl referenceframe.Frame, logger golog.Logger, iter int, solveEpsilon float64) (*NloptIK, error)

CreateNloptIKSolver creates an nloptIK object that can perform gradient descent on metrics for Frames. The parameters are the Frame on which Transform() will be called, a logger, and the number of iterations to run. If the iteration count is less than 1, it will be set to the default of 5000.

func (*NloptIK) Frame 

func (ik *NloptIK) Frame() referenceframe.Frame

Frame returns the associated referenceframe.

func (*NloptIK) GenerateRandomPositions 

func (ik *NloptIK) GenerateRandomPositions(randSeed *rand.Rand) []referenceframe.Input

GenerateRandomPositions generates a random set of positions within the limits of this solver.

func (*NloptIK) Solve 

func (ik *NloptIK) Solve(ctx context.Context,
	c chan<- []referenceframe.Input,
	seed []referenceframe.Input,
	m StateMetric,
	rseed int,
) error

Solve runs the actual solver and sends any solutions found to the given channel.

type Segment added in v0.2.34 

type Segment struct {
	StartPosition      spatial.Pose
	EndPosition        spatial.Pose
	StartConfiguration []referenceframe.Input
	EndConfiguration   []referenceframe.Input
	Frame              referenceframe.Frame
}

Segment contains all the information a constraint needs to determine validity for a movement. It contains the starting inputs, the ending inputs, corresponding poses, and the frame it refers to. Pose fields may be empty, and may be filled in by a constraint that needs them.

type SegmentConstraint added in v0.2.34 

type SegmentConstraint func(*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 SegmentMetric added in v0.2.34 

type SegmentMetric func(*Segment) float64

SegmentMetric are functions which produce some score given an Segment. Lower is better. This is used to sort produced IK solutions by goodness, for example.

type State added in v0.2.34 

type State struct {
	Position      spatial.Pose
	Configuration []referenceframe.Input
	Frame         referenceframe.Frame
}

State contains all the information a constraint needs to determine validity for a movement. It contains the starting inputs, the ending inputs, corresponding poses, and the frame it refers to. Pose fields may be empty, and may be filled in by a constraint that needs them.

type StateConstraint added in v0.2.34 

type StateConstraint func(*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 NewOctreeCollisionConstraint added in v0.2.34 

func NewOctreeCollisionConstraint(octree *pointcloud.BasicOctree, threshold int, buffer 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.

type StateMetric added in v0.2.34 

type StateMetric func(*State) float64

StateMetric are functions which, given a State, produces some score. Lower is better. This is used for gradient descent to converge upon a goal pose, for example.

func CombineMetrics 

func CombineMetrics(metrics ...StateMetric) StateMetric

CombineMetrics will take a variable number of Metrics and return a new Metric which will combine all given metrics into one, summing their distances.

func NewPoseFlexOVMetric 

func NewPoseFlexOVMetric(goal spatial.Pose, alpha float64) StateMetric

NewPoseFlexOVMetric will provide a distance function which will converge on a pose with an OV within an arclength of `alpha` of the ov of the goal given.

func NewPositionOnlyMetric 

func NewPositionOnlyMetric(goal spatial.Pose) StateMetric

NewPositionOnlyMetric returns a Metric that reports the point-wise distance between two poses without regard for orientation. This is useful for scenarios where there are not enough DOF to control orientation, but arbitrary spatial points may still be arrived at.

func NewSquaredNormMetric 

func NewSquaredNormMetric(goal spatial.Pose) StateMetric

NewSquaredNormMetric is the default distance function between two poses to be used for gradient descent.

func NewZeroMetric added in v0.2.26 

func NewZeroMetric() StateMetric

NewZeroMetric always returns zero as the distance between two points.

Source Files

View all Source files

Directories

Path Synopsis
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.