motionplan

package
v0.19.1 Latest Latest
Warning

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

 Go to latest Published: Jan 24, 2024 License: AGPL-3.0 Imports: 28 Imported by: 1

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,
	currentPosition spatialmath.Pose,
	currentInputs []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.

func PlanStepsToGeoPoses added in v0.19.0 

func PlanStepsToGeoPoses(
	planSteps []PlanStep,
	componentName resource.Name,
	origin spatial.GeoPose,
) []spatial.GeoPose

PlanStepsToGeoPoses converts the relative poses the robot will move to into geo poses.

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

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

Plan describes a motion plan.

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

func (Plan) Evaluate added in v0.12.0 

func (plan Plan) Evaluate(distFunc ik.SegmentMetric) (totalCost float64)

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

func (Plan) GetFrameSteps added in v0.9.0 

func (plan Plan) GetFrameSteps(frameName string) ([][]referenceframe.Input, error)

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

func (Plan) String added in v0.9.0 

func (plan Plan) String() string

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

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 PlanStep added in v0.19.0 

type PlanStep map[resource.Name]spatialmath.Pose

PlanStep represents a single step of the plan Describes the pose each resource described by the plan should move to at that step.

func PlanToPlanSteps added in v0.19.0 

func PlanToPlanSteps(
	plan Plan,
	componentName resource.Name,
	planRequest PlanRequest,
	startPose spatial.Pose,
) ([]PlanStep, error)

PlanToPlanSteps converts a plan to the relative poses the robot will move to (relative to the origin).

func (PlanStep) ToProto added in v0.19.0 

func (s PlanStep) ToProto() *pb.PlanStep

ToProto converts a Step to a *pb.PlanStep.

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 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,
) (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 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.

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.