motionplan

package
v0.0.6 Latest Latest
Warning

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

 Go to latest Published: Aug 15, 2022 License: AGPL-3.0 Imports: 23 Imported by: 0

README

Kinematics

This provides various models and software to determine the forward/inverse kinematics positions of a variety of robot arms.

Contents

  • models contains the WRL and XML files that specify various robot arms
  • go-kin contains a kinematics lobrary capable of parsing the above model files and performing forward and inverse kinematics on them
  • Maybe other things that didn't exist when I last updated this file
Models

This tutorial can mostly be followed to create a new robot model, assuming you already have some sort of 3d model: https://www.roboticslibrary.org/tutorials/create-a-robot-model/

go-kin

This is a 100% Go port of Robotics Library. Currently all that has been implemented is part of mdl and math, enough to load a model and calculate forward and inverse kinematics. Dynamics are currently out of scope.

Installation

go get github.com/viamrobotics/kinematics/

Operation

// Load the model file
m, _ := ParseFile("/home/peter/Documents/echo/kinematics/models/mdl/wx250s.xml")

// Initialize the IK solver with the model
ik := CreateJacobianIKSolver(m)

// ForwardPosition will calculate the forward kinematic position based on current joint positions
// By default joint positions will all be zero
m.ForwardPosition()

// Print out the x,y,z,Rx,Ry,Rz position of the End-Effector (EE) in question (in this case 0)
fmt.Println(m.Get6dPosition(0))

// Get the current position of EE0 as a Transform object, and add that to be a goal for EE0
ik.AddGoal(m.GetOperationalPosition(0), 0)

// Create a new list of joint positions
newPos := []float64{1.1, 0.1, 1.3, 0, 0, -1}
// Set joints to the new positions
m.SetPosition(newPos)
// Calculate the forward kinematic position with the new positions
m.ForwardPosition()
// Print the new 6d position with the new joint angles
fmt.Println(m.Get6dPosition(0))

// Try to solve the joint angles to get us back to the original position that was added to the Goals
// Will set joint angles if successful
yay := ik.Solve()
// Print whether the Solve was successful
fmt.Println("yay? ", yay)
// Calculate forward position
m.ForwardPosition()
// Print the new 6d position- should be very close to the one above
fmt.Println(m.Get6dPosition(0))

// Print the []float64 list of current joint positions in radians
fmt.Println(m.GetPosition())

Currently this has only been tested with 6dof robots.

Documentation

Overview

Package motionplan is a motion planning library.

Index

Constants

This section is empty.

Variables

This section is empty.

Functions

func ComputePosition 

func ComputePosition(model referenceframe.Frame, joints *pb.JointPositions) (*commonpb.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 GetSteps 

func GetSteps(seedPos, goalPos spatial.Pose, stepSize float64) int

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

func L2Distance(q1, q2 []float64) float64

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

func NewLineConstraint 

func NewLineConstraint(pt1, pt2 r3.Vector, epsilon 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. epsilon refers to the closeness to the line necessary to be a valid pose.

func NewLinearInterpolatingConstraint 

func NewLinearInterpolatingConstraint(from, to spatial.Pose, epsilon float64) (Constraint, Metric)

NewLinearInterpolatingConstraint creates a constraint function from an arbitrary function that will decide if a given pose is valid. This function will check the given function at each point in checkSeq, and 1-point. If all constraints are satisfied, it will return true. If any intermediate pose violates the constraint, will return false. This constraint will interpolate between the start and end poses, and ensure that the pose given by interpolating the inputs the same amount does not deviate by more than a set amount.

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 NewSlerpOrientationConstraint 

func NewSlerpOrientationConstraint(start, goal spatial.Pose, epsilon 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 epsilon distance of the shortest arc between the two orientations, as well as a metric which returns the distance to that valid region.

func RunPlannerWithWaypoints 

func RunPlannerWithWaypoints(ctx context.Context,
	planner MotionPlanner,
	goals []spatial.Pose,
	seed []frame.Input,
	opts []*PlannerOptions,
	iter int,
) ([][]frame.Input, error)

RunPlannerWithWaypoints will plan to each of a list of goals in oder, optionally also taking a new planner option for each goal.

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 CollisionEntities 

type CollisionEntities interface {
	// contains filtered or unexported methods
}

CollisionEntities defines an interface for a set of collisionEntities that can be treated as a single batch.

func NewSpaceCollisionEntities 

func NewSpaceCollisionEntities(geometries map[string]spatial.Geometry) (CollisionEntities, error)

NewSpaceCollisionEntities is a constructor for spaceCollisionEntities.

type CollisionSystem 

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

CollisionSystem is an object that checks for and records collisions between CollisionEntities.

func NewCollisionSystem 

func NewCollisionSystem(key *ObjectCollisionEntities, optional []CollisionEntities) (*CollisionSystem, error)

NewCollisionSystem creates a new collision system that checks for collisions between the entities in the key CollisionEntities and the entities in each of the optional CollisionEntities.

func NewCollisionSystemFromReference 

func NewCollisionSystemFromReference(
	key *ObjectCollisionEntities,
	optional []CollisionEntities,
	reference *CollisionSystem,
) (*CollisionSystem, error)

NewCollisionSystemFromReference creates a new collision system that checks for collisions between the entities in the key CollisionEntities and the entities in each of the optional CollisionEntities a reference CollisionSystem can also be specified, and edges between entities that exist in this reference system will not be duplicated in the newly constructed system.

func (*CollisionSystem) CollisionBetween 

func (cs *CollisionSystem) CollisionBetween(keyName, testName string) bool

CollisionBetween returns a bool describing if a collision between the two named entities was reported in the CollisionSystem.

func (*CollisionSystem) Collisions 

func (cs *CollisionSystem) Collisions() []Collision

Collisions returns a list of all the reported collisions in the CollisionSystem.

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,
) 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 NewCollisionConstraint 

func NewCollisionConstraint(frame referenceframe.Frame, obstacles, interactionSpaces map[string]spatial.Geometry) Constraint

NewCollisionConstraint is a helper function for creating a collision Constraint that takes a frame and geometries representing obstacles and interaction spaces and will construct a collision avoidance constraint from them.

func NewCollisionConstraintFromWorldState 

func NewCollisionConstraintFromWorldState(
	model referenceframe.Frame,
	fs referenceframe.FrameSystem,
	worldState *commonpb.WorldState,
	observationInput map[string][]referenceframe.Input,
) (Constraint, error)

NewCollisionConstraintFromWorldState creates a collision constraint from a world state, framesystem, a model and a set of initial states.

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 InverseKinematics 

type InverseKinematics interface {
	// Solve receives a context, the goal arm position, and current joint angles.
	Solve(ctx context.Context, c chan<- []referenceframe.Input, goal spatial.Pose, seed []referenceframe.Input, m Metric) 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.

type MotionPlanner 

type MotionPlanner interface {
	// 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
	Plan(context.Context, *commonpb.Pose, []frame.Input, *PlannerOptions) ([][]frame.Input, error)
	Resolution() float64 // Resolution specifies how narrowly to check for constraints
	Frame() frame.Frame  // Frame will return the frame used for planning
}

MotionPlanner provides an interface to path planning methods, providing ways to request a path to be planned, and management of the constraints used to plan paths.

func NewCBiRRTMotionPlanner 

func NewCBiRRTMotionPlanner(frame referenceframe.Frame, nCPU int, logger golog.Logger) (MotionPlanner, error)

NewCBiRRTMotionPlanner creates a cBiRRTMotionPlanner object.

func NewCBiRRTMotionPlannerWithSeed 

func NewCBiRRTMotionPlannerWithSeed(frame referenceframe.Frame, nCPU int, seed *rand.Rand, logger golog.Logger) (MotionPlanner, error)

NewCBiRRTMotionPlannerWithSeed creates a cBiRRTMotionPlanner object with a user specified random seed.

func NewRRTConnectMotionPlanner 

func NewRRTConnectMotionPlanner(frame referenceframe.Frame, nCPU int, seed *rand.Rand, logger golog.Logger) (MotionPlanner, error)

NewRRTConnectMotionPlanner creates a rrtConnectMotionPlanner object.

func NewRRTConnectMotionPlannerWithSeed 

func NewRRTConnectMotionPlannerWithSeed(frame referenceframe.Frame, nCPU int, seed *rand.Rand, logger golog.Logger) (MotionPlanner, error)

NewRRTConnectMotionPlannerWithSeed creates a rrtConnectMotionPlanner object with a user specified random seed.

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() []referenceframe.Input

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

func (*NloptIK) SetSeed 

func (ik *NloptIK) SetSeed(seed int64)

SetSeed sets the random seed 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,
) error

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

type ObjectCollisionEntities 

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

ObjectCollisionEntities is an implementation of CollisionEntities for entities that occupy physical space and should not be intersected it is exported because the key CollisionEntities in a CollisionSystem must be of this type.

func NewObjectCollisionEntities 

func NewObjectCollisionEntities(geometries map[string]spatial.Geometry) (*ObjectCollisionEntities, error)

NewObjectCollisionEntities is a constructor for ObjectCollisionEntities, an exported implementation of CollisionEntities.

type PlannerOptions 

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

PlannerOptions are a set of options to be passed to a planner which will specify how to solve a motion planning problem.

func DefaultConstraint 

func DefaultConstraint(
	from, to spatial.Pose,
	f frame.Frame,
	opt *PlannerOptions,
) *PlannerOptions

DefaultConstraint creates a default constraint and metric that constrains the position and orientation. The allowed magnitude of deviation of the position and orientation from the start or goal shall never be greater than than the magnitude of deviation between the start and goal poses. For example- if a user requests a translation, orientation will not change during the movement. If there is an obstacle, deflection from the ideal path is allowed as a function of the length of the ideal path.

func NewDefaultPlannerOptions 

func NewDefaultPlannerOptions() *PlannerOptions

NewDefaultPlannerOptions specifies a set of default options for the planner.

func (*PlannerOptions) AddConstraint 

func (c *PlannerOptions) AddConstraint(name string, cons Constraint)

AddConstraint 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 (*PlannerOptions) CheckConstraintPath 

func (c *PlannerOptions) CheckConstraintPath(ci *ConstraintInput, resolution float64) (bool, *ConstraintInput)

CheckConstraintPath will interpolate between two joint inputs and check that `true` is returned for all constraints in all intermediate positions. If failing on an intermediate position, it will return that position.

func (*PlannerOptions) CheckConstraints 

func (c *PlannerOptions) CheckConstraints(cInput *ConstraintInput) (bool, float64)

CheckConstraints will check a given input against all constraints.

func (*PlannerOptions) Clone 

func (p *PlannerOptions) Clone() *PlannerOptions

Clone makes a deep copy of the PlannerOptions.

func (*PlannerOptions) Constraints 

func (c *PlannerOptions) Constraints() []string

Constraints will list all constraints by name.

func (*PlannerOptions) RemoveConstraint 

func (c *PlannerOptions) RemoveConstraint(name string)

RemoveConstraint will remove the given constraint.

func (*PlannerOptions) SetMaxSolutions 

func (p *PlannerOptions) SetMaxSolutions(maxSolutions int)

SetMaxSolutions sets the maximum number of IK solutions to generate for the planner.

func (*PlannerOptions) SetMetric 

func (p *PlannerOptions) SetMetric(m Metric)

SetMetric sets the distance metric for the solver.

func (*PlannerOptions) SetMinScore 

func (p *PlannerOptions) SetMinScore(minScore float64)

SetMinScore specifies the IK stopping score for the planner.

func (*PlannerOptions) SetPathDist 

func (p *PlannerOptions) SetPathDist(m Metric)

SetPathDist sets the distance metric for the solver to move a constraint-violating point into a valid manifold.

type SolvableFrameSystem 

type SolvableFrameSystem struct {
	frame.FrameSystem
	// contains filtered or unexported fields
}

SolvableFrameSystem wraps a FrameSystem to allow solving between frames of the frame system. Note that this needs to live in motionplan, not referenceframe, to avoid circular dependencies.

func NewSolvableFrameSystem 

func NewSolvableFrameSystem(fs frame.FrameSystem, logger golog.Logger) *SolvableFrameSystem

NewSolvableFrameSystem will create a new solver for a frame system.

func (*SolvableFrameSystem) SetPlannerGen 

func (fss *SolvableFrameSystem) SetPlannerGen(mpFunc func(frame.Frame, int, golog.Logger) (MotionPlanner, error))

SetPlannerGen sets the function which is used to create the motion planner to solve a requested plan. A SolvableFrameSystem wraps a complete frame system, and will make solverFrames on the fly to solve for. These solverFrames are used to create the planner here.

func (*SolvableFrameSystem) SolvePose 

func (fss *SolvableFrameSystem) SolvePose(ctx context.Context,
	seedMap map[string][]frame.Input,
	goal spatial.Pose,
	solveFrameName, goalFrameName string,
) ([]map[string][]frame.Input, error)

SolvePose will take a set of starting positions, a goal frame, a frame to solve for, and a pose. The function will then try to path plan the full frame system such that the solveFrame has the goal pose from the perspective of the goalFrame. For example, if a world system has a gripper attached to an arm attached to a gantry, and the system was being solved to place the gripper at a particular pose in the world, the solveFrame would be the gripper and the goalFrame would be the world frame. It will use the default planner options.

func (*SolvableFrameSystem) SolvePoseWithOptions 

func (fss *SolvableFrameSystem) SolvePoseWithOptions(ctx context.Context,
	seedMap map[string][]frame.Input,
	goal spatial.Pose,
	solveFrameName, goalFrameName string,
	worldState *commonpb.WorldState,
	opt *PlannerOptions,
) ([]map[string][]frame.Input, error)

SolvePoseWithOptions will take a set of starting positions, a goal frame, a frame to solve for, a pose, and a configurable set of PlannerOptions. It will solve the solveFrame to the goal pose with respect to the goal frame using the provided planning options.

func (*SolvableFrameSystem) SolveWaypointsWithOptions 

func (fss *SolvableFrameSystem) SolveWaypointsWithOptions(ctx context.Context,
	seedMap map[string][]frame.Input,
	goals []spatial.Pose,
	solveFrameName, goalFrameName string,
	worldState *commonpb.WorldState,
	opts []*PlannerOptions,
) ([]map[string][]frame.Input, error)

SolveWaypointsWithOptions will take a set of starting positions, a goal frame, a frame to solve for, goal poses, and a configurable set of PlannerOptions. It will solve the solveFrame to the goal poses with respect to the goal frame using the provided planning options.

Source Files

View all Source files

Directories

Path Synopsis
Package visualization provides a minimal way to see from robot's perspective
 Package visualization provides a minimal way to see from robot's perspective

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.