motion

package
v0.40.0 Latest Latest
Warning

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

 Go to latest Published: Sep 3, 2024 License: AGPL-3.0 Imports: 23 Imported by: 3

Documentation

Overview

Package motion is the service that allows you to plan and execute movements.

Index

Constants

View Source 
const (
	// PlanStateUnspecified denotes an the Plan is in an unspecified state. This should never happen.
	PlanStateUnspecified = iota

	// PlanStateInProgress denotes an the Plan is in an in progress state. It is a temporary state.
	PlanStateInProgress

	// PlanStateStopped denotes an the Plan is in a stopped state. It is a terminal state.
	PlanStateStopped

	// PlanStateSucceeded denotes an the Plan is in a succeeded state. It is a terminal state.
	PlanStateSucceeded

	// PlanStateFailed denotes an the Plan is in a failed state. It is a terminal state.
	PlanStateFailed
)
View Source 
const SubtypeName = "motion"

SubtypeName is the name of the type of service.

Variables

View Source 
var API = resource.APINamespaceRDK.WithServiceType(SubtypeName)

API is a variable that identifies the motion service resource API.

View Source 
var ErrGoalWithinPlanDeviation = errors.New("no need to move, already within planDeviationMM")

ErrGoalWithinPlanDeviation is an error describing when planning fails because there is nothing to be done.

View Source 
var SLAMOrientationAdjustment = spatialmath.NewPoseFromOrientation(&spatialmath.OrientationVectorDegrees{OZ: 1, Theta: -90})

SLAMOrientationAdjustment is needed because a SLAM map pose has orientation of OZ=1, Theta=0 when the rover is intended to be pointing at the +X axis of the SLAM map. However, for a rover's relative planning frame, driving forwards increments +Y. Thus we must adjust where the rover thinks it is.

View Source 
var TerminalStateSet = map[PlanState]struct{}{
	PlanStateStopped:   {},
	PlanStateSucceeded: {},
	PlanStateFailed:    {},
}

TerminalStateSet is a set that defines the PlanState values which are terminal i.e. which represent the end of a plan.

Functions

func MoveArm added in v0.34.0 

func MoveArm(ctx context.Context, logger logging.Logger, a arm.Arm, dst spatialmath.Pose) error

MoveArm is a helper function to abstract away movement for general arms.

func Named added in v0.0.8 

func Named(name string) resource.Name

Named is a helper for getting the named motion service's typed resource name.

func NewRPCServiceServer added in v0.2.36 

func NewRPCServiceServer(coll resource.APIResourceCollection[Service]) interface{}

NewRPCServiceServer constructs a motion gRPC service server. It is intentionally untyped to prevent use outside of tests.

func PollHistoryUntilSuccessOrError added in v0.17.0 

func PollHistoryUntilSuccessOrError(
	ctx context.Context,
	m Service,
	interval time.Duration,
	req PlanHistoryReq,
) error

PollHistoryUntilSuccessOrError polls `PlanHistory()` with `req` every `interval` until a terminal state is reached. An error is returned if the terminal state is Failed, Stopped or an invalid state or if the context has an error. nil is returned if the terminal state is Succeeded.

Types

type ExecutionID added in v0.15.0 

type ExecutionID = uuid.UUID

ExecutionID uniquely identifies an execution.

type ListPlanStatusesReq added in v0.11.0 

type ListPlanStatusesReq struct {
	// If true then only active plans will be returned.
	OnlyActivePlans bool
	Extra           map[string]interface{}
}

ListPlanStatusesReq describes the request to ListPlanStatuses().

type Localizer added in v0.3.0 

type Localizer interface {
	CurrentPosition(context.Context) (*referenceframe.PoseInFrame, error)
}

Localizer is an interface which both slam and movementsensor can satisfy when wrapped respectively.

func NewMovementSensorLocalizer added in v0.6.0 

func NewMovementSensorLocalizer(ms movementsensor.MovementSensor, origin *geo.Point, calibration spatialmath.Pose) Localizer

NewMovementSensorLocalizer creates a Localizer from a MovementSensor. An origin point must be specified and the localizer will return Poses relative to this point. A calibration pose can also be specified, which will adjust the location after it is calculated relative to the origin.

func NewSLAMLocalizer added in v0.6.0 

func NewSLAMLocalizer(slam slam.Service) Localizer

NewSLAMLocalizer creates a new Localizer that relies on a slam service to report Pose.

func TwoDLocalizer added in v0.21.0 

func TwoDLocalizer(l Localizer) Localizer

TwoDLocalizer will check the orientation of the pose of a localizer, and ensure that it is normal to the XY plane. If it is not, it will be altered such that it is (accounting for e.g. an ourdoor base with one wheel on a rock). If the orientation is such that the base is pointed directly up or down (or is upside-down), an error is returned. The alteration to ensure normality to the plane is done by transforming the (0,1,0) vector by the provided orientation, and then using atan2 on the new x and y values to determine the vector of travel that would be followed.

type MotionConfiguration added in v0.7.3 

type MotionConfiguration struct {
	ObstacleDetectors     []ObstacleDetectorName
	PositionPollingFreqHz *float64
	ObstaclePollingFreqHz *float64
	PlanDeviationMM       float64
	LinearMPerSec         float64
	AngularDegsPerSec     float64
}

MotionConfiguration specifies how to configure a call.

type MoveOnGlobeReq added in v0.11.0 

type MoveOnGlobeReq struct {
	// ComponentName of the component to move
	ComponentName resource.Name
	// Goal destination the component should be moved to
	Destination *geo.Point
	// Heading the component should have a when it reaches the goal.
	// Range [0-360] Left Hand Rule (N: 0, E: 90, S: 180, W: 270)
	Heading float64
	// Name of the momement sensor which can be used to derive Position & Heading
	MovementSensorName resource.Name
	// Static obstacles that should be navigated around
	Obstacles []*spatialmath.GeoGeometry
	// Set of bounds which the robot must remain within while navigating
	BoundingRegions []*spatialmath.GeoGeometry
	// Optional motion configuration
	MotionCfg *MotionConfiguration
	Extra     map[string]interface{}
}

MoveOnGlobeReq describes the request to the MoveOnGlobe interface method.

func (MoveOnGlobeReq) String added in v0.16.0 

func (r MoveOnGlobeReq) String() string

type MoveOnMapReq added in v0.15.0 

type MoveOnMapReq struct {
	ComponentName resource.Name
	Destination   spatialmath.Pose
	SlamName      resource.Name
	MotionCfg     *MotionConfiguration
	Obstacles     []spatialmath.Geometry
	Extra         map[string]interface{}
}

MoveOnMapReq describes a request to MoveOnMap.

func (MoveOnMapReq) String added in v0.20.0 

func (r MoveOnMapReq) String() string

type ObstacleDetectorName added in v0.11.0 

type ObstacleDetectorName struct {
	VisionServiceName resource.Name
	CameraName        resource.Name
}

ObstacleDetectorName pairs a vision service name with a camera name.

type PlanHistoryReq added in v0.11.0 

type PlanHistoryReq struct {
	// ComponentName the returned plans should be associated with.
	ComponentName resource.Name
	// When true, only the most recent plan will be returned which matches the ComponentName & ExecutionID if one was provided.
	LastPlanOnly bool
	// Optional, when not uuid.Nil it specifies the ExecutionID of the plans that should be returned.
	// Can be used to query plans from executions before the most recent one.
	ExecutionID ExecutionID
	Extra       map[string]interface{}
}

PlanHistoryReq describes the request to PlanHistory().

type PlanID added in v0.15.0 

type PlanID = uuid.UUID

PlanID uniquely identifies a Plan.

type PlanState added in v0.11.0 

type PlanState uint8

PlanState denotes the state a Plan is in.

func (PlanState) String added in v0.11.0 

func (ps PlanState) String() string

func (PlanState) ToProto added in v0.11.0 

func (ps PlanState) ToProto() pb.PlanState

ToProto converts a PlanState to a pb.PlanState.

type PlanStatus added in v0.11.0 

type PlanStatus struct {
	State     PlanState
	Timestamp time.Time
	Reason    *string
}

PlanStatus describes the state of a given plan at a point in time allong with an optional reason why the PlanStatus transitioned to that state.

func (PlanStatus) ToProto added in v0.11.0 

func (ps PlanStatus) ToProto() *pb.PlanStatus

ToProto converts a PlanStatus to a *pb.PlanStatus.

type PlanStatusWithID added in v0.11.0 

type PlanStatusWithID struct {
	PlanID        PlanID
	ComponentName resource.Name
	ExecutionID   ExecutionID
	Status        PlanStatus
}

PlanStatusWithID describes the state of a given plan at a point in time plus the PlanId, ComponentName and ExecutionID the status is associated with.

func (PlanStatusWithID) ToProto added in v0.11.0 

func (ps PlanStatusWithID) ToProto() *pb.PlanStatusWithID

ToProto converts a PlanStatusWithID to a *pb.PlanStatusWithID.

type PlanWithMetadata added in v0.21.0 

type PlanWithMetadata struct {
	// Unique ID of the plan
	ID PlanID
	// Name of the component the plan is planning for
	ComponentName resource.Name
	// Unique ID of the execution
	ExecutionID ExecutionID
	// The motionplan itself
	motionplan.Plan
	// The GPS point to anchor visualized plans at
	AnchorGeoPose *spatialmath.GeoPose
}

PlanWithMetadata represents a motion plan with additional metadata used by the motion service.

func (PlanWithMetadata) Renderable added in v0.21.0 

func (p PlanWithMetadata) Renderable() PlanWithMetadata

Renderable returns a copy of the struct substituting its Plan for a GeoPlan consisting of smuggled global coordinates This will only be done if the AnchorGeoPose field is non-nil, otherwise the original struct will be returned.

func (PlanWithMetadata) ToProto added in v0.21.0 

func (p PlanWithMetadata) ToProto() *pb.Plan

ToProto converts a Plan to a *pb.Plan.

type PlanWithStatus added in v0.11.0 

type PlanWithStatus struct {
	Plan          PlanWithMetadata
	StatusHistory []PlanStatus
}

PlanWithStatus contains a plan, its current status, and all state changes that came prior sorted by ascending timestamp.

func (PlanWithStatus) ToProto added in v0.11.0 

func (pws PlanWithStatus) ToProto() *pb.PlanWithStatus

ToProto converts a PlanWithStatus to a *pb.PlanWithStatus.

type Service 

type Service interface {
	resource.Resource

	// Move is the primary method to move multiple components or any object to a specified location.
	// Given a destination pose and a component, Move constructs a kinematic chain from goal to destination,
	// solves it while adhering to constraints, and executes the movement to avoid collisions with the machine itself and other known objects.
	Move(
		ctx context.Context,
		componentName resource.Name,
		destination *referenceframe.PoseInFrame,
		worldState *referenceframe.WorldState,
		constraints *motionplan.Constraints,
		extra map[string]interface{},
	) (bool, error)

	// MoveOnMap moves a base component to a destination Pose on a SLAM map and returns a unique ExecutionID.
	// If the machine is already within PlanDeviationM of the goal, an error is returned.
	// Monitor progress with `GetPlan()` and `ListPlanStatuses()`, and check the machine's position via the SLAM service.
	// Designed for autonomous indoor navigation of rover bases.
	MoveOnMap(
		ctx context.Context,
		req MoveOnMapReq,
	) (ExecutionID, error)

	// MoveOnGlobe moves a base component to a destination GPS point(latitude, longitude and returns a unique ExecutionID.
	// If the machine is already within PlanDeviationM of the goal, an error is returned.
	// This non-blocking method uses a movement sensor to verify the location of the base.
	// You can monitor progress with `GetPlan()` and `ListPlanStatuses()`. Designed for autonomous GPS navigation of rover bases.
	MoveOnGlobe(
		ctx context.Context,
		req MoveOnGlobeReq,
	) (ExecutionID, error)

	// GetPose returns the location and orientation of a component within a frame system.
	// It returns a `PoseInFrame` describing the pose of the specified component relative to the specified destination frame.
	// The `supplemental_transforms` argument can be used to augment the machine's existing frame system with additional frames.
	GetPose(
		ctx context.Context,
		componentName resource.Name,
		destinationFrame string,
		supplementalTransforms []*referenceframe.LinkInFrame,
		extra map[string]interface{},
	) (*referenceframe.PoseInFrame, error)

	// StopPlan stops a base component being moved by an in progress `MoveOnGlobe()` or `MoveOnMap()` call.
	StopPlan(
		ctx context.Context,
		req StopPlanReq,
	) error

	// ListPlanStatuses returns the statuses of plans created by `MoveOnGlobe()` or `MoveOnMap()` since the motion service initialized.
	// It includes plans that are in progress or have changed state in the last 24 hours.
	// All repeated fields are in chronological order.
	ListPlanStatuses(
		ctx context.Context,
		req ListPlanStatusesReq,
	) ([]PlanStatusWithID, error)

	// PlanHistory returns the plan history of the most recent `MoveOnGlobe()` or `MoveOnMap()` call by default.
	// The history for earlier executions can be requested by providing an ExecutionID.
	// It returns a result if the execution is active or has changed state in the last 24 hours and the machine has not reinitialized.
	// Plans never change; replans always create new plans and replans share the ExecutionID of the previously executing plan.
	PlanHistory(
		ctx context.Context,
		req PlanHistoryReq,
	) ([]PlanWithStatus, error)
}

A Service controls the flow of moving components.

Move example: 

motionService, err := motion.FromRobot(machine, "builtin")

// Assumes a gripper configured with name "my_gripper" on the machine
gripperName := gripper.Named("my_gripper")

// Define a destination Pose
destination := referenceframe.NewPoseInFrame("world", spatialmath.NewPoseFromPoint(r3.Vector{X: 0.1, Y: 0.0, Z: 0.0}))

// Create obstacles
boxPose := spatialmath.NewPoseFromPoint(r3.Vector{X: 0.0, Y: 0.0, Z: 0.0})
boxDims := r3.Vector{X: 0.2, Y: 0.2, Z: 0.2} // 20cm x 20cm x 20cm box
obstacle, _ := spatialmath.NewBox(boxPose, boxDims, "obstacle_1")

geometryInFrame := referenceframe.NewGeometriesInFrame("base", []spatialmath.Geometry{obstacle})
obstacles := []*referenceframe.GeometriesInFrame{geometryInFrame}

// Create transforms
transform := referenceframe.NewLinkInFrame("gripper",
	spatialmath.NewPoseFromPoint(r3.Vector{X: 0.1, Y: 0.0, Z: 0.1}), "transform_1", nil
)
transforms := []*referenceframe.LinkInFrame{transform}

// Create WorldState
worldState, err := referenceframe.NewWorldState(obstacles, transforms)

// Move gripper component
moved, err := motionService.Move(context.Background(), gripperName, destination, worldState, nil, nil)

MoveOnMap example: 

// Assumes a base with the name "my_base" is configured on the machine
myBaseResourceName := base.Named("my_base")
mySLAMServiceResourceName := slam.Named("my_slam_service")

// Define a destination Pose
myPose := spatialmath.NewPoseFromPoint(r3.Vector{Y: 10})

// Move the base component to the destination pose
executionID, err := motionService.MoveOnMap(context.Background(), motion.MoveOnMapReq{
	ComponentName: myBaseResourceName,
	Destination:   myPose,
	SlamName:      mySLAMServiceResourceName,
})

// MoveOnMap is a non-blocking method and this line can optionally be added to block until the movement is done
err = motion.PollHistoryUntilSuccessOrError(
	context.Background(),
	motionService,
	time.Duration(time.Second),
	motion.PlanHistoryReq{
		ComponentName: myBaseResourceName,
		ExecutionID:   executionID,
	},
)

MoveOnGlobe example: 

// Assumes a base with the name "myBase" is configured on the machine
// Get the resource names of the base and movement sensor
myBaseResourceName := base.Named("myBase")
myMvmntSensorResourceName := movementsensor.Named("my_movement_sensor")

// Define a destination Point at the GPS coordinates [0, 0]
myDestination := geo.NewPoint(0, 0)

// Move the base component to the designated geographic location, as reported by the movement sensor
executionID, err := motionService.MoveOnGlobe(context.Background(), motion.MoveOnGlobeReq{
	ComponentName:      myBaseResourceName,
	Destination:        myDestination,
	MovementSensorName: myMvmntSensorResourceName,
})

// Assumes there is an active MoveOnMap() or MoveonGlobe() in progress for myBase
//	MoveOnGlobe is a non-blocking method and this line can optionally be added to block until the movement is done
err = motion.PollHistoryUntilSuccessOrError(
	context.Background(),
	motionService,
	time.Duration(time.Second),
	motion.PlanHistoryReq{
		ComponentName: myBaseResourceName,
		ExecutionID:   executionID,
	},
)

GetPose example: 

// Insert code to connect to your machine.
// (see CONNECT tab of your machine's page in the Viam app)

// Assumes a gripper configured with name "my_gripper" on the machine
gripperName := gripper.Named("my_gripper")

// Access the motion service
motionService, err := motion.FromRobot(machine, "builtin")
if err != nil {
  logger.Fatal(err)
}

myArmMotionPose, err := motionService.GetPose(context.Background(), my_gripper, referenceframe.World, nil, nil)
if err != nil {
  logger.Fatal(err)
}
logger.Info("Position of myArm from the motion service:", myArmMotionPose.Pose().Point())
logger.Info("Orientation of myArm from the motion service:", myArmMotionPose.Pose().Orientation())

StopPlan example: 

motionService, err := motion.FromRobot(machine, "builtin")
myBaseResourceName := base.Named("myBase")

myMvmntSensorResourceName := movement_sensor.Named("my_movement_sensor")
myDestination := geo.NewPoint(0, 0)

// Assuming a `MoveOnGlobe()`` started the execution
// Stop the base component which was instructed to move by `MoveOnGlobe()` or `MoveOnMap()`
err := motionService.StopPlan(context.Background(), motion.StopPlanReq{
    ComponentName: s.req.ComponentName,
})

ListPlanStatuses example: 

motionService, err := motion.FromRobot(machine, "builtin")

// Get the plan(s) of the base component's most recent execution i.e. `MoveOnGlobe()` or `MoveOnMap()` call.
planStatuses, err := motionService.ListPlanStatuses(context.Background(), motion.ListPlanStatusesReq{})

PlanHistory example: 

// Get the resource name of the base component
myBaseResourceName := base.Named("myBase")

// Get the plan history of the base component's most recent execution (e.g., MoveOnGlobe or MoveOnMap call)
planHistory, err := motionService.PlanHistory(context.Background(), motion.PlanHistoryReq{
	ComponentName: myBaseResourceName,
})

func FromDependencies added in v0.2.47 

func FromDependencies(deps resource.Dependencies, name string) (Service, error)

FromDependencies is a helper for getting the named motion service from a collection of dependencies.

func FromRobot 

func FromRobot(r robot.Robot, name string) (Service, error)

FromRobot is a helper for getting the named motion service from the given Robot.

func NewClientFromConn 

func NewClientFromConn(
	ctx context.Context,
	conn rpc.ClientConn,
	remoteName string,
	name resource.Name,
	logger logging.Logger,
) (Service, error)

NewClientFromConn constructs a new Client from connection passed in.

type StopPlanReq added in v0.11.0 

type StopPlanReq struct {
	// ComponentName of the plan which should be stopped
	ComponentName resource.Name
	Extra         map[string]interface{}
}

StopPlanReq describes the request to StopPlan().

Source Files

View all Source files

Directories

Path Synopsis
Package builtin implements a motion service.
 Package builtin implements a motion service.
state
Package state provides apis for motion builtin plan executions and manages the state of those executions
 Package state provides apis for motion builtin plan executions and manages the state of those executions
Package explore implements a motion service for exploration.
 Package explore implements a motion service for exploration.
Package register registers all relevant motion services and API specific functions.
 Package register registers all relevant motion services and API specific functions.

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.