motionplan

package
v0.2.27 Latest Latest
Warning

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

 Go to latest Published: Mar 20, 2023 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"
)

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 Constraint) (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 L2Distance 

func L2Distance(q1, q2 []float64) float64

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

func NewAbsoluteLinearInterpolatingConstraint added in v0.0.8 

func NewAbsoluteLinearInterpolatingConstraint(from, to spatial.Pose, linTol, orientTol float64) (Constraint, Metric)

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) (Constraint, Metric)

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) (Constraint, Metric)

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 arc 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) (Constraint, Metric)

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) (Constraint, Metric)

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 arc 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 PlanRobotMotion added in v0.0.8 

func PlanRobotMotion(ctx context.Context,
	dst *frame.PoseInFrame,
	f frame.Frame,
	r robot.Robot,
	fs frame.FrameSystem,
	worldState *frame.WorldState,
	constraintSpec *pb.Constraints,
	planningOpts map[string]interface{},
) ([]map[string][]frame.Input, error)

PlanRobotMotion plans a motion to destination for a given frame. A robot object is passed in and current position inputs are determined.

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) (*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,
	newGoal spatialmath.Pose,
	seed []referenceframe.Input,
	m Metric,
	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 Constraint 

type Constraint func(*ConstraintInput) (bool, float64)

Constraint defines functions able to determine whether or not a given position is valid. TODO (pl): Determine how Gradient should fit into this A bool returning whether the given input is known to be good, and a float representing how far the input is from "ideal".

func NewJointConstraint 

func NewJointConstraint(threshold float64) Constraint

NewJointConstraint returns a constraint which will sum the squared differences in each input from start to end It will return false if that sum is over a specified threshold.

func NewOrientationConstraint 

func NewOrientationConstraint(orientFunc func(o spatial.Orientation) bool) Constraint

NewOrientationConstraint returns a constraint which will return false if the startPos or endPos orientations are not valid.

type ConstraintInput 

type ConstraintInput struct {
	StartPos   spatial.Pose
	EndPos     spatial.Pose
	StartInput []referenceframe.Input
	EndInput   []referenceframe.Input
	Frame      referenceframe.Frame
}

ConstraintInput 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 DubinPathAttr added in v0.0.7 

type DubinPathAttr struct {
	TotalLen   float64   // Total length of all segments making up a single Dubin's path
	DubinsPath []float64 // Length array of the six possible Dubin's path combiantions
	Straight   bool      // True if Dubin's Path segment is straight
}

DubinPathAttr describes a Dubins path that can be taken from one point to another.

func GetDubinTrajectoryFromPath added in v0.0.7 

func GetDubinTrajectoryFromPath(waypoints [][]referenceframe.Input, d Dubins) []DubinPathAttr

GetDubinTrajectoryFromPath takes a path of waypoints that can be followed using Dubins paths and returns a list of DubinPathAttrs describing the Dubins paths to get between waypoints.

type Dubins added in v0.0.7 

type Dubins struct {
	Radius          float64 // Turning radius of car
	PointSeparation float64 // Separation of points on path to check for collision
}

Dubins describes the parameters for a specific Dubin's problem.

func NewDubins added in v0.0.7 

func NewDubins(radius, pointSeparation float64) (*Dubins, error)

NewDubins creates a new Dubins instance given a valid radius and point separation.

func (*Dubins) AllPaths added in v0.0.7 

func (d *Dubins) AllPaths(start, end []float64, sorts bool) []DubinPathAttr

AllPaths finds every possible Dubins path from start to end and returns them as DubinPathAttrs.

func (*Dubins) DubinsPath added in v0.0.7 

func (d *Dubins) DubinsPath(start, end []float64) [][]float64

DubinsPath returns a list of points along the shortest Dubins path from start to end.

type DubinsRRTMotionPlanner added in v0.0.7 

type DubinsRRTMotionPlanner struct {
	D Dubins
	// contains filtered or unexported fields
}

DubinsRRTMotionPlanner an object able to solve for paths using Dubin's Car Model around obstacles to some goal for a given referenceframe. It uses the RRT* with vehicle dynamics algorithm, Khanal 2022 https://arxiv.org/abs/2206.10533

func NewDubinsRRTMotionPlanner added in v0.0.7 

func NewDubinsRRTMotionPlanner(frame referenceframe.Frame, nCPU int, logger golog.Logger, d Dubins) (*DubinsRRTMotionPlanner, error)

NewDubinsRRTMotionPlanner creates a DubinsRRTMotionPlanner object.

func (*DubinsRRTMotionPlanner) Frame added in v0.0.7 

func (mp *DubinsRRTMotionPlanner) Frame() referenceframe.Frame

Frame will return the frame used for planning.

func (*DubinsRRTMotionPlanner) Plan added in v0.0.7 

func (mp *DubinsRRTMotionPlanner) Plan(ctx context.Context,
	goal spatialmath.Pose,
	seed []referenceframe.Input,
	planOpts *plannerOptions,
) ([][]referenceframe.Input, error)

Plan will take a context, a goal position, and an input start state and return a series of state waypoints which should be visited in order to arrive at the goal while satisfying all constraints.

func (*DubinsRRTMotionPlanner) Resolution added in v0.0.7 

func (mp *DubinsRRTMotionPlanner) Resolution() float64

Resolution specifies how narrowly to check for constraints.

type InverseKinematics 

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

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

type Metric 

type Metric func(spatial.Pose, spatial.Pose) float64

Metric defines a distance function to be minimized by gradient descent algorithms.

func CombineMetrics 

func CombineMetrics(metrics ...Metric) Metric

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

NewPoseFlexOVMetric will provide a distance function which will converge on an OV within an arclength of `alpha` of the ov of the goal given. The 3d point of the goal given is discarded, and the function will converge on the 3d point of the `to` pose (this is probably what you want).

func NewPositionOnlyMetric 

func NewPositionOnlyMetric() Metric

NewPositionOnlyMetric returns a Metric that reports the point-wise distance between two poses.

func NewSquaredNormMetric 

func NewSquaredNormMetric() Metric

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

func NewZeroMetric added in v0.2.26 

func NewZeroMetric() Metric

NewZeroMetric always returns zero as the distance between two points.

type NloptIK 

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

NloptIK TODO.

func CreateNloptIKSolver 

func CreateNloptIKSolver(mdl referenceframe.Frame, logger golog.Logger, iter int) (*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,
	newGoal spatial.Pose,
	seed []referenceframe.Input,
	m Metric,
	rseed int,
) error

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

Source Files

View all Source files

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.