motionplan

Documentation

Overview

Package motionplan is a motion planning library.

Index

• Constants
• func CheckPlan(checkFrame frame.Frame, plan Plan, worldState *frame.WorldState, ...) error
• func ComputeOOBPosition(model referenceframe.Frame, joints *pb.JointPositions) (spatialmath.Pose, error)
• func ComputePosition(model referenceframe.Frame, joints *pb.JointPositions) (spatialmath.Pose, error)
• func EvaluatePlan(plan [][]referenceframe.Input, distFunc ik.SegmentMetric) (totalCost float64)
• func L2Distance(q1, q2 []float64) float64
• func PathStepCount(seedPos, goalPos spatialmath.Pose, stepSize float64) int
• func PlanFrameMotion(ctx context.Context, logger golog.Logger, dst spatialmath.Pose, f frame.Frame, ...) ([][]frame.Input, error)
• type Collision
• type ConstraintHandler
  • func (c *ConstraintHandler) AddSegmentConstraint(name string, cons SegmentConstraint)
  • func (c *ConstraintHandler) AddStateConstraint(name string, cons StateConstraint)
  • func (c *ConstraintHandler) CheckSegmentAndStateValidity(segment *ik.Segment, resolution float64) (bool, *ik.Segment)
  • func (c *ConstraintHandler) CheckSegmentConstraints(segment *ik.Segment) (bool, string)
  • func (c *ConstraintHandler) CheckStateConstraints(state *ik.State) (bool, string)
  • func (c *ConstraintHandler) CheckStateConstraintsAcrossSegment(ci *ik.Segment, resolution float64) (bool, *ik.Segment)
  • func (c *ConstraintHandler) RemoveSegmentConstraint(name string)
  • func (c *ConstraintHandler) RemoveStateConstraint(name string)
  • func (c *ConstraintHandler) SegmentConstraints() []string
  • func (c *ConstraintHandler) StateConstraints() []string
• type Plan
  • func PlanMotion(ctx context.Context, request *PlanRequest) (Plan, error)
  • func (plan Plan) GetFrameSteps(frameName string) ([][]referenceframe.Input, error)
  • func (plan Plan) String() string
• type PlanRequest
• type SegmentConstraint
• type StateConstraint
  • func NewAbsoluteLinearInterpolatingConstraint(from, to spatial.Pose, linTol, orientTol float64) (StateConstraint, ik.StateMetric)
  • func NewLineConstraint(pt1, pt2 r3.Vector, tolerance float64) (StateConstraint, ik.StateMetric)
  • func NewOctreeCollisionConstraint(octree *pointcloud.BasicOctree, threshold int, buffer float64) StateConstraint
  • func NewPlaneConstraint(pNorm, pt r3.Vector, writingAngle, epsilon float64) (StateConstraint, ik.StateMetric)
  • func NewProportionalLinearInterpolatingConstraint(from, to spatial.Pose, epsilon float64) (StateConstraint, ik.StateMetric)
  • func NewSlerpOrientationConstraint(start, goal spatial.Pose, tolerance float64) (StateConstraint, ik.StateMetric)

Constants

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

the set of supported motion profiles.

Functions

func CheckPlan

func CheckPlan(
	checkFrame frame.Frame,
	plan Plan,
	worldState *frame.WorldState,
	fs frame.FrameSystem,
	currentPosition spatialmath.Pose,
	currentInputs []frame.Input,
	errorState spatialmath.Pose,
	logger *zap.SugaredLogger,
) error

CheckPlan checks if obstacles intersect the trajectory of the frame following the plan.

func ComputeOOBPosition

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

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

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

func L2Distance

func L2Distance(q1, q2 []float64) float64

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

func PathStepCount

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

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.

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

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

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

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

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

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

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

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

func (c *ConstraintHandler) RemoveSegmentConstraint(name string)

RemoveSegmentConstraint will remove the given constraint.

func (*ConstraintHandler) RemoveStateConstraint

func (c *ConstraintHandler) RemoveStateConstraint(name string)

RemoveStateConstraint will remove the given constraint.

func (*ConstraintHandler) SegmentConstraints

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

SegmentConstraints will list all segment constraints by name.

func (*ConstraintHandler) StateConstraints

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

StateConstraints will list all state constraints by name.

type Plan

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

Plan describes a motion plan.

func PlanMotion

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

PlanMotion plans a motion from a provided plan request.

func (Plan) GetFrameSteps

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

func (plan Plan) String() string

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

type PlanRequest

type PlanRequest struct {
	Logger             golog.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

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

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

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

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

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.