motionplan

package
v0.57.0-rc0 Latest Latest
Warning

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

 Go to latest Published: Jan 6, 2025 License: AGPL-3.0 Imports: 26 Imported by: 1

Documentation

Overview

Package motionplan is a motion planning library.

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 CalculateFrameErrorState added in v0.33.0 

func CalculateFrameErrorState(e ExecutionState, executionFrame, localizationFrame referenceframe.Frame) (spatialmath.Pose, error)

CalculateFrameErrorState takes an ExecutionState and a Frame and calculates the error between the Frame's expected and actual positions.

func CheckPlan added in v0.9.0 

func CheckPlan(
	checkFrame referenceframe.Frame,
	executionState ExecutionState,
	worldState *referenceframe.WorldState,
	fs referenceframe.FrameSystem,
	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 NewAlgAndConstraintMismatchErr added in v0.56.0 

func NewAlgAndConstraintMismatchErr(planAlg string) error

NewAlgAndConstraintMismatchErr is returned when an incompatible planning_alg is specified and there are contraints.

func PathStateCount added in v0.56.0 

func PathStateCount(seedPos, goalPos spatialmath.Pose, stepSize float64) int

PathStateCount 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 referenceframe.Frame,
	seed []referenceframe.Input,
	constraints *Constraints,
	planningOpts map[string]interface{},
) ([][]referenceframe.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 CollisionSpecification added in v0.29.0 

type CollisionSpecification struct {
	// Pairs of frame which should be allowed to collide with one another
	Allows []CollisionSpecificationAllowedFrameCollisions
}

CollisionSpecification is used to selectively apply obstacle avoidance to specific parts of the robot.

type CollisionSpecificationAllowedFrameCollisions added in v0.29.0 

type CollisionSpecificationAllowedFrameCollisions struct {
	Frame1, Frame2 string
}

CollisionSpecificationAllowedFrameCollisions is used to define frames that are allowed to collide.

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) AddSegmentFSConstraint added in v0.54.0 

func (c *ConstraintHandler) AddSegmentFSConstraint(name string, cons SegmentFSConstraint)

AddSegmentFSConstraint 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) AddStateFSConstraint added in v0.54.0 

func (c *ConstraintHandler) AddStateFSConstraint(name string, cons StateFSConstraint)

AddStateFSConstraint 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) CheckSegmentAndStateValidityFS added in v0.54.0 

func (c *ConstraintHandler) CheckSegmentAndStateValidityFS(segment *ik.SegmentFS, resolution float64) (bool, *ik.SegmentFS)

CheckSegmentAndStateValidityFS will check a 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) CheckSegmentFSConstraints added in v0.54.0 

func (c *ConstraintHandler) CheckSegmentFSConstraints(segment *ik.SegmentFS) (bool, string)

CheckSegmentFSConstraints will check a given input against all FS 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) CheckStateConstraintsAcrossSegmentFS added in v0.54.0 

func (c *ConstraintHandler) CheckStateConstraintsAcrossSegmentFS(ci *ik.SegmentFS, resolution float64) (bool, *ik.SegmentFS)

CheckStateConstraintsAcrossSegmentFS will interpolate the given input from the StartConfiguration to the EndConfiguration, 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 SegmentFS representing the valid portion of the segment, if any. If no part of the segment is valid, then `false, nil` is returned.

func (*ConstraintHandler) CheckStateFSConstraints added in v0.54.0 

func (c *ConstraintHandler) CheckStateFSConstraints(state *ik.StateFS) (bool, string)

CheckStateFSConstraints will check a given input against all FS state constraints. Return values are: -- a bool representing whether all constraints passed -- if failing, a string naming the failed constraint.

func (*ConstraintHandler) RemoveSegmentConstraint added in v0.2.34 

func (c *ConstraintHandler) RemoveSegmentConstraint(name string)

RemoveSegmentConstraint will remove the given constraint.

func (*ConstraintHandler) RemoveSegmentFSConstraint added in v0.54.0 

func (c *ConstraintHandler) RemoveSegmentFSConstraint(name string)

RemoveSegmentFSConstraint 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) RemoveStateFSConstraint added in v0.54.0 

func (c *ConstraintHandler) RemoveStateFSConstraint(name string)

RemoveStateFSConstraint 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) SegmentFSConstraints added in v0.54.0 

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

SegmentFSConstraints will list all FS 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.

func (*ConstraintHandler) StateFSConstraints added in v0.54.0 

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

StateFSConstraints will list all FS state constraints by name.

type Constraints added in v0.29.0 

type Constraints struct {
	LinearConstraint       []LinearConstraint
	PseudolinearConstraint []PseudolinearConstraint
	OrientationConstraint  []OrientationConstraint
	CollisionSpecification []CollisionSpecification
}

Constraints is a struct to store the constraints imposed upon a robot It serves as a convenenient RDK wrapper for the protobuf object.

func ConstraintsFromProtobuf added in v0.29.0 

func ConstraintsFromProtobuf(pbConstraint *motionpb.Constraints) *Constraints

ConstraintsFromProtobuf converts a protobuf object to a Constraints object.

func NewConstraints added in v0.29.0 

func NewConstraints(
	linConstraints []LinearConstraint,
	pseudoConstraints []PseudolinearConstraint,
	orientConstraints []OrientationConstraint,
	collSpecifications []CollisionSpecification,
) *Constraints

NewConstraints initializes a Constraints object with user-defined LinearConstraint, OrientationConstraint, and CollisionSpecification.

func NewEmptyConstraints added in v0.29.0 

func NewEmptyConstraints() *Constraints

NewEmptyConstraints creates a new, empty Constraints object.

func (*Constraints) AddCollisionSpecification added in v0.29.0 

func (c *Constraints) AddCollisionSpecification(collConstraint CollisionSpecification)

AddCollisionSpecification appends a CollisionSpecification to a Constraints object.

func (*Constraints) AddLinearConstraint added in v0.29.0 

func (c *Constraints) AddLinearConstraint(linConstraint LinearConstraint)

AddLinearConstraint appends a LinearConstraint to a Constraints object.

func (*Constraints) AddOrientationConstraint added in v0.29.0 

func (c *Constraints) AddOrientationConstraint(orientConstraint OrientationConstraint)

AddOrientationConstraint appends a OrientationConstraint to a Constraints object.

func (*Constraints) AddPseudolinearConstraint added in v0.54.0 

func (c *Constraints) AddPseudolinearConstraint(plinConstraint PseudolinearConstraint)

AddPseudolinearConstraint appends a PseudolinearConstraint to a Constraints object.

func (*Constraints) GetCollisionSpecification added in v0.30.0 

func (c *Constraints) GetCollisionSpecification() []CollisionSpecification

GetCollisionSpecification checks if the Constraints object is nil and if not then returns its CollisionSpecification field.

func (*Constraints) GetLinearConstraint added in v0.30.0 

func (c *Constraints) GetLinearConstraint() []LinearConstraint

GetLinearConstraint checks if the Constraints object is nil and if not then returns its LinearConstraint field.

func (*Constraints) GetOrientationConstraint added in v0.30.0 

func (c *Constraints) GetOrientationConstraint() []OrientationConstraint

GetOrientationConstraint checks if the Constraints object is nil and if not then returns its OrientationConstraint field.

func (*Constraints) GetPseudolinearConstraint added in v0.54.0 

func (c *Constraints) GetPseudolinearConstraint() []PseudolinearConstraint

GetPseudolinearConstraint checks if the Constraints object is nil and if not then returns its PseudolinearConstraint field.

func (*Constraints) ToProtobuf added in v0.29.0 

func (c *Constraints) ToProtobuf() *motionpb.Constraints

ToProtobuf takes an existing Constraints object and converts it to a protobuf.

type ExecutionState added in v0.27.0 

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

ExecutionState describes a plan and a particular state along it.

func NewExecutionState added in v0.27.0 

func NewExecutionState(
	plan Plan,
	index int,
	currentInputs map[string][]referenceframe.Input,
	currentPose PathState,
) (ExecutionState, error)

NewExecutionState will construct an ExecutionState struct.

func (*ExecutionState) CurrentInputs added in v0.27.0 

func (e *ExecutionState) CurrentInputs() map[string][]referenceframe.Input

CurrentInputs returns the current inputs of the components associated with the ExecutionState.

func (*ExecutionState) CurrentPoses added in v0.27.0 

func (e *ExecutionState) CurrentPoses() PathState

CurrentPoses returns the current poses in frame of the components associated with the ExecutionState.

func (*ExecutionState) Index added in v0.27.0 

func (e *ExecutionState) Index() int

Index returns the currently-executing index of the execution state's Plan.

func (*ExecutionState) Plan added in v0.27.0 

func (e *ExecutionState) Plan() Plan

Plan returns the plan associated with the execution state.

type LinearConstraint added in v0.29.0 

type LinearConstraint struct {
	LineToleranceMm          float64 // Max linear deviation from straight-line between start and goal, in mm.
	OrientationToleranceDegs float64
}

LinearConstraint specifies that the components being moved should move linearly relative to their goals.

type OrientationConstraint added in v0.29.0 

type OrientationConstraint struct {
	OrientationToleranceDegs float64
}

OrientationConstraint specifies that the components being moved will not deviate orientation beyond some threshold.

type Path added in v0.21.0 

type Path []PathState

Path is a slice of PathStates describing a series of Poses for a robot to travel to in the course of following a Plan. The pose of the PathState is the pose at the end of the corresponding set of inputs in the Trajectory.

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 PathState added in v0.56.0 

type PathState map[string]*referenceframe.PoseInFrame

PathState is a mapping of Frame names to PoseInFrames.

func ComputePathStateFromConfiguration added in v0.56.0 

func ComputePathStateFromConfiguration(fs referenceframe.FrameSystem, configuration map[string][]referenceframe.Input) (PathState, error)

ComputePathStateFromConfiguration computes the poses for each frame in a framesystem in frame of World, using the provided configuration.

func PathStateFromProto added in v0.56.0 

func PathStateFromProto(ps *pb.PlanStep) (PathState, error)

PathStateFromProto converts a *pb.PlanStep to a PlanStep.

func (PathState) ToProto added in v0.56.0 

func (ps PathState) ToProto() *pb.PlanStep

ToProto converts a PathState 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
	// The planner will hit each Goal in order. Each goal may be a configuration or PathState for holonomic motion, or must be a
	// PathState for non-holonomic motion. For holonomic motion, if both a configuration and PathState are given, an error is thrown.
	// TODO: Perhaps we could do something where some components are enforced to arrive at a certain configuration, but others can have IK
	// run to solve for poses. Doing this while enforcing configurations may be tricky.
	Goals       []*PlanState
	FrameSystem referenceframe.FrameSystem

	// This must always have a configuration filled in, for geometry placement purposes.
	// If poses are also filled in, the configuration will be used to determine geometry collisions, but the poses will be used
	// in IK to generate plan start configurations. The given configuration will NOT automatically be added to the seed tree.
	// The use case here is that if a particularly difficult path must be planned between two poses, that can be done first to ensure
	// feasibility, and then other plans can be requested to connect to that returned plan's configurations.
	StartState      *PlanState
	WorldState      *referenceframe.WorldState
	BoundingRegions []spatialmath.Geometry
	Constraints     *Constraints
	Options         map[string]interface{}
}

PlanRequest is a struct to store all the data necessary to make a call to PlanMotion.

type PlanState added in v0.56.0 

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

PlanState is a struct which holds both a PathState and a configuration. This is intended to be used as start or goal states for plans. Either field may be nil.

func DeserializePlanState added in v0.56.0 

func DeserializePlanState(iface map[string]interface{}) (*PlanState, error)

DeserializePlanState turns a serialized PlanState back into a PlanState.

func NewPlanState added in v0.56.0 

func NewPlanState(poses PathState, configuration map[string][]referenceframe.Input) *PlanState

NewPlanState creates a PlanState from the given poses and configuration. Either or both may be nil.

func (*PlanState) ComputePoses added in v0.56.0 

func (p *PlanState) ComputePoses(fs referenceframe.FrameSystem) (PathState, error)

ComputePoses returns the poses of a PlanState if they are populated, or computes them using the given FrameSystem if not.

func (*PlanState) Configuration added in v0.56.0 

func (p *PlanState) Configuration() map[string][]referenceframe.Input

Configuration returns the configuration of the PlanState.

func (*PlanState) Poses added in v0.56.0 

func (p *PlanState) Poses() PathState

Poses returns the poses of the PlanState.

func (PlanState) Serialize added in v0.56.0 

func (p PlanState) Serialize() map[string]interface{}

Serialize turns a PlanState into a map[string]interface suitable for being transmitted over proto.

type PseudolinearConstraint added in v0.54.0 

type PseudolinearConstraint struct {
	LineToleranceFactor        float64
	OrientationToleranceFactor float64
}

PseudolinearConstraint specifies that the component being moved should not deviate from the straight-line path to their goal by more than a factor proportional to the distance from start to goal. For example, if a component is moving 100mm, then a LineToleranceFactor of 1.0 means that the component will remain within a 100mm radius of the straight-line start-goal path.

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 SegmentFSConstraint added in v0.54.0 

type SegmentFSConstraint func(*ik.SegmentFS) bool

SegmentFSConstraint 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 NewBoundingRegionConstraint added in v0.29.0 

func NewBoundingRegionConstraint(robotGeoms, boundingRegions []spatial.Geometry, collisionBufferMM float64) StateConstraint

NewBoundingRegionConstraint will determine if the given list of robot geometries are in collision with the given list of bounding regions.

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 geometries 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,
	linEpsilon, orientEpsilon 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 StateFSConstraint added in v0.54.0 

type StateFSConstraint func(*ik.StateFS) bool

StateFSConstraint tests whether a given robot configuration is valid If the returned bool is true, the constraint is satisfied and the state is valid.

func CreateAbsoluteLinearInterpolatingConstraintFS added in v0.54.0 

func CreateAbsoluteLinearInterpolatingConstraintFS(
	fs referenceframe.FrameSystem,
	startCfg map[string][]referenceframe.Input,
	from, to PathState,
	linTol, orientTol float64,
) (StateFSConstraint, ik.StateFSMetric, error)

CreateAbsoluteLinearInterpolatingConstraintFS 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 CreateLineConstraintFS added in v0.54.0 

func CreateLineConstraintFS(
	fs referenceframe.FrameSystem,
	startCfg map[string][]referenceframe.Input,
	from, to PathState,
	tolerance float64,
) (StateFSConstraint, ik.StateFSMetric, error)

CreateLineConstraintFS will measure the linear distance between the positions of two poses across a frame system, and return a constraint that checks whether given positions are within a specified tolerance distance of the shortest line segment between their respective positions, as well as a metric which returns the distance to that valid region.

func CreateProportionalLinearInterpolatingConstraintFS added in v0.54.0 

func CreateProportionalLinearInterpolatingConstraintFS(
	fs referenceframe.FrameSystem,
	startCfg map[string][]referenceframe.Input,
	from, to PathState,
	linTol, orientTol float64,
) (StateFSConstraint, ik.StateFSMetric, error)

CreateProportionalLinearInterpolatingConstraintFS will provide the same metric and constraint as CreateAbsoluteLinearInterpolatingConstraintFS, except that allowable linear and orientation deviation is scaled based on the distance from start to goal.

func CreateSlerpOrientationConstraintFS added in v0.54.0 

func CreateSlerpOrientationConstraintFS(
	fs referenceframe.FrameSystem,
	startCfg map[string][]referenceframe.Input,
	from, to PathState,
	tolerance float64,
) (StateFSConstraint, ik.StateFSMetric, error)

CreateSlerpOrientationConstraintFS will measure the orientation difference between the orientation of two poses across a frame system, and return a constraint that returns whether given orientations are within a given tolerance distance of the shortest segment between their respective orientations, as well as a metric which returns the distance to that valid region.

func NewCollisionConstraintFS added in v0.54.0 

func NewCollisionConstraintFS(
	moving, static []spatial.Geometry,
	collisionSpecifications []*Collision,
	reportDistances bool,
	collisionBufferMM float64,
) (StateFSConstraint, error)

NewCollisionConstraintFS is the most general method to create a collision constraint for a frame system, 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.

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.SegmentFSMetric) 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.