motionplan

package
v0.0.9 Latest Latest
Warning

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

 Go to latest Published: Sep 9, 2022 License: AGPL-3.0 Imports: 26 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 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 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 *commonpb.WorldState,
	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 *commonpb.WorldState,
	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.

func PlanWaypoints added in v0.0.8 

func PlanWaypoints(ctx context.Context,
	logger golog.Logger,
	dst []*frame.PoseInFrame,
	f frame.Frame,
	seedMap map[string][]frame.Input,
	fs frame.FrameSystem,
	worldState *commonpb.WorldState,
	planningOpts []map[string]interface{},
) ([]map[string][]frame.Input, error)

PlanWaypoints plans motions to a list of destinations in order for a given frame. It takes a given frame system, wraps it with a SolvableFS, and solves. It will generate a list of intermediate waypoints as well to pass to the solvable framesystem if possible.

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 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 (*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 *commonpb.Pose,
	seed []referenceframe.Input,
	opt *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(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)
	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 NewDubinsRRTMotionPlanner added in v0.0.7 

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

NewDubinsRRTMotionPlanner creates a DubinsRRTMotionPlanner object.

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 {

	// For the below values, if left uninitialized, default values will be used. To disable, set < 0
	// Max number of ik solutions to consider
	MaxSolutions int `json:"max_ik_solutions"`
	// Movements that score below this amount are considered "good enough" and returned immediately
	MinScore float64 `json:"min_ik_score"`
	// Check constraints are still met every this many mm/degrees of movement.
	Resolution float64 `json:"resolution"`
	// 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 NewBasicPlannerOptions added in v0.0.8 

func NewBasicPlannerOptions() *PlannerOptions

NewBasicPlannerOptions specifies a set of basic 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) 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 *frame.PoseInFrame,
	solveFrameName 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) SolveWaypointsWithOptions 

func (fss *SolvableFrameSystem) SolveWaypointsWithOptions(ctx context.Context,
	seedMap map[string][]frame.Input,
	goals []*frame.PoseInFrame,
	solveFrameName string,
	worldState *commonpb.WorldState,
	motionConfigs []map[string]interface{},
) ([]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

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.