spatialmath

package
v0.32.1 Latest Latest
Warning

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

 Go to latest Published: Jul 10, 2024 License: AGPL-3.0 Imports: 13 Imported by: 26

Documentation

Overview

Package spatialmath defines spatial mathematical operations. Poses represent a position in 6 degrees of freedom, i.e. a position and an orientation. Positions are represented as r3 Vectors, while Orientations are an interface able to be represented many different ways. This package provides various Orientation implementations as well as the ability to perform a variety of useful operations on Poses and Orientations.

Index

Constants

View Source 
const (
	UnknownType = GeometryType("")
	BoxType     = GeometryType("box")
	SphereType  = GeometryType("sphere")
	CapsuleType = GeometryType("capsule")
	PointType   = GeometryType("point")
)

The set of allowed representations for orientation.

View Source 
const (
	NoOrientationType            = OrientationType("")
	OrientationVectorDegreesType = OrientationType("ov_degrees")
	OrientationVectorRadiansType = OrientationType("ov_radians")
	EulerAnglesType              = OrientationType("euler_angles")
	AxisAnglesType               = OrientationType("axis_angles")
	QuaternionType               = OrientationType("quaternion")
)

The set of allowed representations for orientation.

Variables

This section is empty.

Functions

func ClosestPointSegmentPoint added in v0.2.13 

func ClosestPointSegmentPoint(pt1, pt2, query r3.Vector) r3.Vector

ClosestPointSegmentPoint takes a line segment defined by pt1 and pt2, plus some query point, and returns the point on the line segment which is closest to the query point.

func ClosestPointsSegmentSegment added in v0.2.13 

func ClosestPointsSegmentSegment(ap1, ap2, bp1, bp2 r3.Vector) (r3.Vector, r3.Vector)

ClosestPointsSegmentSegment will return the points at which two line segments are closest to one another.

func DistToLineSegment added in v0.2.13 

func DistToLineSegment(pt1, pt2, query r3.Vector) float64

DistToLineSegment takes a line segment defined by pt1 and pt2, plus some query point, and returns the cartesian distance from the query point to the closest point on the line segment.

func Flip 

func Flip(q quat.Number) quat.Number

Flip will multiply a quaternion by -1, returning a quaternion representing the same orientation but in the opposing octant.

func GeoGeometryToProtobuf added in v0.28.0 

func GeoGeometryToProtobuf(geoObst *GeoGeometry) *commonpb.GeoGeometry

GeoGeometryToProtobuf converts the GeoGeometry struct into an equivalent Protobuf message.

func GeoPointToPoint added in v0.15.0 

func GeoPointToPoint(point, origin *geo.Point) r3.Vector

GeoPointToPoint returns the point (r3.Vector) which translates the origin to the destination geopoint Because the function we use to project a point on a spheroid to a plane is nonlinear, we linearize it about a specified origin point.

func GeometriesAlmostEqual added in v0.24.0 

func GeometriesAlmostEqual(a, b Geometry) bool

GeometriesAlmostEqual returns a bool describing if the two input Geometries are equal.

func GetCartesianDistance added in v0.3.0 

func GetCartesianDistance(p, q *geo.Point) (float64, float64)

GetCartesianDistance calculates the latitude and longitide displacement between p and q in millimeters. Note that this is an approximation since we are trying to project a point on a sphere onto a plane. The closer these points are the more accurate the approximation is.

func NewGeometriesToProto added in v0.3.0 

func NewGeometriesToProto(geometries []Geometry) []*commonpb.Geometry

NewGeometriesToProto converts a list of Geometries to profobuf.

func Norm 

func Norm(q quat.Number) float64

Norm returns the norm of the quaternion, i.e. the sqrt of the sum of the squares of the imaginary parts.

func Normalize 

func Normalize(q quat.Number) quat.Number

Normalize a quaternion, returning its, versor (unit quaternion).

func OffsetBy 

func OffsetBy(a, b *commonpb.Pose) *commonpb.Pose

OffsetBy takes two offsets and computes the final position.

func OrientationAlmostEqual 

func OrientationAlmostEqual(o1, o2 Orientation) bool

OrientationAlmostEqual will return a bool describing whether 2 poses have approximately the same orientation.

func OrientationAlmostEqualEps added in v0.2.1 

func OrientationAlmostEqualEps(o1, o2 Orientation, epsilon float64) bool

OrientationAlmostEqualEps will return a bool describing whether 2 poses have approximately the same orientation.

func PlaneNormal added in v0.2.13 

func PlaneNormal(p0, p1, p2 r3.Vector) r3.Vector

PlaneNormal returns the plane normal of the triangle defined by the three given points.

func PoseAlmostCoincident 

func PoseAlmostCoincident(a, b Pose) bool

PoseAlmostCoincident will return a bool describing whether 2 poses approximately are at the same 3D coordinate location. This uses the same epsilon as the default value for the Viam IK solver.

func PoseAlmostCoincidentEps 

func PoseAlmostCoincidentEps(a, b Pose, epsilon float64) bool

PoseAlmostCoincidentEps will return a bool describing whether 2 poses approximately are at the same 3D coordinate location. This uses a passed in epsilon value.

func PoseAlmostEqual 

func PoseAlmostEqual(a, b Pose) bool

PoseAlmostEqual will return a bool describing whether 2 poses are approximately the same.

func PoseAlmostEqualEps added in v0.2.1 

func PoseAlmostEqualEps(a, b Pose, epsilon float64) bool

PoseAlmostEqualEps will return a bool describing whether 2 poses are approximately the same.

func PoseMap 

func PoseMap(p Pose) (map[string]interface{}, error)

PoseMap encodes the orientation interface to something serializable and human readable.

func PoseToProtobuf 

func PoseToProtobuf(p Pose) *commonpb.Pose

PoseToProtobuf converts a pose to the pose format protobuf expects (which is as OrientationVectorDegrees).

func QuatToR3AA 

func QuatToR3AA(q quat.Number) r3.Vector

QuatToR3AA converts a quat to an R3 axis angle in the same way the C++ Eigen library does. https://eigen.tuxfamily.org/dox/AngleAxis_8h_source.html

func QuaternionAlmostEqual 

func QuaternionAlmostEqual(a, b quat.Number, tol float64) bool

QuaternionAlmostEqual is an equality test for all the float components of a quaternion. Quaternions have double coverage, q == -q, and this function will *not* account for this. Use OrientationAlmostEqual unless you're certain this is what you want.

func R3VectorAlmostEqual 

func R3VectorAlmostEqual(a, b r3.Vector, epsilon float64) bool

R3VectorAlmostEqual compares two r3.Vector objects and returns if the all elementwise differences are less than epsilon.

func ResetPoseDQTranslation added in v0.1.2 

func ResetPoseDQTranslation(p Pose, v r3.Vector)

ResetPoseDQTranslation takes a Pose that must be a dualQuaternion and reset's it's translation.

func SegmentDistanceToSegment added in v0.2.13 

func SegmentDistanceToSegment(ap1, ap2, bp1, bp2 r3.Vector) float64

SegmentDistanceToSegment will compute the distance separating two line segments at their closest point.

Types

type AngularVelocity 

type AngularVelocity r3.Vector

AngularVelocity contains angular velocity in deg/s across x/y/z axes.

func EulerToAngVel added in v0.1.2 

func EulerToAngVel(diffEu EulerAngles, dt float64) AngularVelocity

EulerToAngVel calculates an angular velocity based on an orientation change expressed in euler angles over a time differnce.

func OrientationToAngularVel added in v0.1.2 

func OrientationToAngularVel(diff Orientation, dt float64) AngularVelocity

OrientationToAngularVel calculates an angular velocity based on an orientation change over a time differnce.

func PointAngVel added in v0.1.2 

func PointAngVel(r, v r3.Vector) AngularVelocity

PointAngVel returns the angular velocity using the defiition r X v / |r|.

func R3ToAngVel added in v0.1.2 

func R3ToAngVel(vec r3.Vector) *AngularVelocity

R3ToAngVel converts an r3Vector to an angular velocity.

func (*AngularVelocity) MulAngVel added in v0.1.2 

func (av *AngularVelocity) MulAngVel(t float64) AngularVelocity

MulAngVel scales the angular velocity by a single scalar value.

type AxisConfig 

type AxisConfig r3.Vector

AxisConfig represents the configuration format representing an axis.

func NewAxisConfig 

func NewAxisConfig(axis R4AA) *AxisConfig

NewAxisConfig constructs a config from an R4AA.

func (AxisConfig) ParseConfig 

func (a AxisConfig) ParseConfig() R4AA

ParseConfig converts an AxisConfig into an R4AA object.

type EulerAngles 

type EulerAngles struct {
	Roll  float64 `json:"roll"`  // phi, X
	Pitch float64 `json:"pitch"` // theta, Y
	Yaw   float64 `json:"yaw"`   // psi, Z
}

EulerAngles are three angles (in radians) used to represent the rotation of an object in 3D Euclidean space The Tait–Bryan angle formalism is used, with rotations around three distinct axes in the z-y′-x″ sequence.

func NewEulerAngles 

func NewEulerAngles() *EulerAngles

NewEulerAngles creates an empty EulerAngles struct.

func QuatToEulerAngles 

func QuatToEulerAngles(q quat.Number) *EulerAngles

QuatToEulerAngles converts a quaternion to the euler angle representation. Algorithm from Wikipedia. https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_angles_conversion

func (*EulerAngles) AxisAngles 

func (ea *EulerAngles) AxisAngles() *R4AA

AxisAngles returns the orientation in axis angle representation.

func (*EulerAngles) EulerAngles 

func (ea *EulerAngles) EulerAngles() *EulerAngles

EulerAngles returns orientation in Euler angle representation.

func (*EulerAngles) OrientationVectorDegrees 

func (ea *EulerAngles) OrientationVectorDegrees() *OrientationVectorDegrees

OrientationVectorDegrees returns orientation as an orientation vector (in degrees).

func (*EulerAngles) OrientationVectorRadians 

func (ea *EulerAngles) OrientationVectorRadians() *OrientationVector

OrientationVectorRadians returns orientation as an orientation vector (in radians).

func (*EulerAngles) Quaternion 

func (ea *EulerAngles) Quaternion() quat.Number

Quaternion returns orientation in quaternion representation.

func (*EulerAngles) RotationMatrix 

func (ea *EulerAngles) RotationMatrix() *RotationMatrix

RotationMatrix returns the orientation in rotation matrix representation.

type GeoGeometry added in v0.28.0 

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

GeoGeometry is a struct to store the location and geometric structure of an obstacle in a geospatial environment.

func GeoGeometriesFromConfig added in v0.28.0 

func GeoGeometriesFromConfig(config *GeoGeometryConfig) ([]*GeoGeometry, error)

GeoGeometriesFromConfig takes a GeoGeometryConfig and returns a list of GeoGeometries.

func GeoGeometriesFromConfigs added in v0.28.0 

func GeoGeometriesFromConfigs(configs []*GeoGeometryConfig) ([]*GeoGeometry, error)

GeoGeometriesFromConfigs takes a GeoGeometryConfig and returns a list of GeoGeometries.

func GeoGeometryFromProtobuf added in v0.28.0 

func GeoGeometryFromProtobuf(protoGeoObst *commonpb.GeoGeometry) (*GeoGeometry, error)

GeoGeometryFromProtobuf takes a Protobuf representation of a GeoGeometry and converts back into a Go struct.

func NewGeoGeometry added in v0.28.0 

func NewGeoGeometry(loc *geo.Point, geom []Geometry) *GeoGeometry

NewGeoGeometry constructs a GeoGeometry from a geo.Point and a slice of Geometries.

func (*GeoGeometry) Geometries added in v0.28.0 

func (gob *GeoGeometry) Geometries() []Geometry

Geometries returns the geometries which comprise structure of the GeoGeometry.

func (*GeoGeometry) Location added in v0.28.0 

func (gob *GeoGeometry) Location() *geo.Point

Location returns the locating coordinates of the GeoGeometry.

type GeoGeometryConfig added in v0.28.0 

type GeoGeometryConfig struct {
	Location   *commonpb.GeoPoint `json:"location"`
	Geometries []*GeometryConfig  `json:"geometries"`
}

GeoGeometryConfig specifies the format of GeoGeometries specified through the configuration file.

func NewGeoGeometryConfig added in v0.28.0 

func NewGeoGeometryConfig(geo *GeoGeometry) (*GeoGeometryConfig, error)

NewGeoGeometryConfig takes a GeoGeometry and returns a GeoGeometryConfig.

type GeoPose added in v0.7.3 

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

GeoPose is a struct to store to location and heading in a geospatial environment.

func NewGeoPose added in v0.7.3 

func NewGeoPose(loc *geo.Point, heading float64) *GeoPose

NewGeoPose constructs a GeoPose from a geo.Point and float64.

func PoseToGeoPose added in v0.15.0 

func PoseToGeoPose(relativeTo *GeoPose, pose Pose) *GeoPose

PoseToGeoPose converts a pose (which are always in mm) into a GeoPose treating relativeTo as the origin.

func (*GeoPose) Heading added in v0.7.3 

func (gpo *GeoPose) Heading() float64

Heading returns a number from [0-360) where 0 is north.

func (*GeoPose) Location added in v0.7.3 

func (gpo *GeoPose) Location() *geo.Point

Location returns the locating coordinates of the GeoPose.

type Geometry 

type Geometry interface {
	// Pose returns the Pose of the center of the Geometry
	Pose() Pose

	// Transform returns a copy of the Geometry that has been transformed by the given Pose
	Transform(Pose) Geometry

	// CollidesWith returns a bool describing if the two geometries are within the given float of colliding with each other.
	CollidesWith(Geometry, float64) (bool, error)

	// If DistanceFrom is negative, it represents the penetration depth of the two geometries, which are in collision.
	// Penetration depth magnitude is defined as the minimum translation which would result in the geometries not colliding.
	// For certain entity pairs (box-box) this may be a conservative estimate of separation distance rather than exact.
	DistanceFrom(Geometry) (float64, error)

	// EncompassedBy returns a bool describing if a given Geometry is completely encompassed by the Geometry passed as an argument.
	EncompassedBy(Geometry) (bool, error)

	// SetLabel sets the name of the geometry
	SetLabel(string)

	// Label returns the name of the geometry
	Label() string

	// ToPoints returns a vector of points that together represent a point cloud of the Geometry
	ToPoints(float64) []r3.Vector

	// ToProtobuf converts a Geometry to its protobuf representation.
	ToProtobuf() *commonpb.Geometry

	json.Marshaler
}

Geometry is an interface defining a 3D solid.

func BoundingSphere added in v0.13.0 

func BoundingSphere(geometry Geometry) (Geometry, error)

BoundingSphere returns a spherical geometry centered on the point (0, 0, 0) that will encompass the given geometry if it were to be rotated 360 degrees about the Z axis. The label of the new geometry is inherited from the given one.

func GeoGeometriesToGeometries added in v0.28.0 

func GeoGeometriesToGeometries(obstacles []*GeoGeometry, origin *geo.Point) []Geometry

GeoGeometriesToGeometries converts a list of GeoGeometries into a list of Geometries.

func NewBox 

func NewBox(pose Pose, dims r3.Vector, label string) (Geometry, error)

NewBox instantiates a new box Geometry.

func NewCapsule added in v0.2.13 

func NewCapsule(offset Pose, radius, length float64, label string) (Geometry, error)

NewCapsule instantiates a new capsule Geometry.

func NewGeometriesFromProto added in v0.3.0 

func NewGeometriesFromProto(proto []*commonpb.Geometry) ([]Geometry, error)

NewGeometriesFromProto converts a list of Geometries from protobuf.

func NewGeometryFromProto 

func NewGeometryFromProto(geometry *commonpb.Geometry) (Geometry, error)

NewGeometryFromProto instantiates a new Geometry from a protobuf Geometry message.

func NewPoint 

func NewPoint(pt r3.Vector, label string) Geometry

NewPoint instantiates a new point Geometry.

func NewSphere 

func NewSphere(offset Pose, radius float64, label string) (Geometry, error)

NewSphere instantiates a new sphere Geometry.

type GeometryConfig 

type GeometryConfig struct {
	Type GeometryType `json:"type"`

	// parameters used for defining a box's rectangular cross-section
	X float64 `json:"x"`
	Y float64 `json:"y"`
	Z float64 `json:"z"`

	// parameter used for defining a sphere's radius'
	R float64 `json:"r"`

	// parameter used for defining a capsule's length
	L float64 `json:"l"`

	// define an offset to position the geometry
	TranslationOffset r3.Vector         `json:"translation,omitempty"`
	OrientationOffset OrientationConfig `json:"orientation,omitempty"`

	Label string
}

GeometryConfig specifies the format of geometries specified through JSON configuration files.

func NewGeometryConfig 

func NewGeometryConfig(g Geometry) (*GeometryConfig, error)

NewGeometryConfig creates a config for a Geometry from an offset Pose.

func (*GeometryConfig) ParseConfig 

func (config *GeometryConfig) ParseConfig() (Geometry, error)

ParseConfig converts a GeometryConfig into the correct GeometryCreator type, as specified in its Type field.

func (*GeometryConfig) ToProtobuf added in v0.2.6 

func (config *GeometryConfig) ToProtobuf() (*commonpb.Geometry, error)

ToProtobuf converts a GeometryConfig to Protobuf.

type GeometryType added in v0.1.0 

type GeometryType string

GeometryType defines what geometry creator representations are known.

type Orientation 

type Orientation interface {
	OrientationVectorRadians() *OrientationVector
	OrientationVectorDegrees() *OrientationVectorDegrees
	AxisAngles() *R4AA
	Quaternion() quat.Number
	EulerAngles() *EulerAngles
	RotationMatrix() *RotationMatrix
}

Orientation is an interface used to express the different parameterizations of the orientation of a rigid object or a frame of reference in 3D Euclidean space.

func NewZeroOrientation 

func NewZeroOrientation() Orientation

NewZeroOrientation returns an orientation which signifies no rotation.

func OrientationBetween 

func OrientationBetween(o1, o2 Orientation) Orientation

OrientationBetween returns the orientation representing the difference between the two given orientations.

func OrientationInverse 

func OrientationInverse(o Orientation) Orientation

OrientationInverse returns the orientation representing the inverse of the given orientation.

type OrientationConfig 

type OrientationConfig struct {
	Type  OrientationType `json:"type"`
	Value map[string]any  `json:"value,omitempty"`
}

OrientationConfig holds the underlying type of orientation, and the value.

func NewOrientationConfig 

func NewOrientationConfig(o Orientation) (*OrientationConfig, error)

NewOrientationConfig encodes the orientation interface to something serializable and human readable.

func (*OrientationConfig) ParseConfig 

func (config *OrientationConfig) ParseConfig() (Orientation, error)

ParseConfig will use the Type in OrientationConfig and convert into the correct struct that implements Orientation.

type OrientationType 

type OrientationType string

OrientationType defines what orientation representations are known.

type OrientationVector 

type OrientationVector struct {
	Theta float64 `json:"th"`
	OX    float64 `json:"x"`
	OY    float64 `json:"y"`
	OZ    float64 `json:"z"`
}

OrientationVector containing ox, oy, oz, theta represents an orientation vector Structured similarly to an angle axis, an orientation vector works differently. Rather than representing an orientation with an arbitrary axis and a rotation around it from an origin, an orientation vector represents orientation such that the ox/oy/oz components represent the point on the cartesian unit sphere at which your end effector is pointing from the origin, and that unit vector forms an axis around which theta rotates. This means that incrementing/decrementing theta will perform an in-line rotation of the end effector. Theta is defined as rotation between two planes: the plane defined by the origin, the point (0,0,1), and the rx,ry,rz point, and the plane defined by the origin, the rx,ry,rz point, and the new local Z axis. So if theta is kept at zero as the north/south pole is circled, the Roll will correct itself to remain in-line.

func NewOrientationVector 

func NewOrientationVector() *OrientationVector

NewOrientationVector Creates a zero-initialized OrientationVector.

func QuatToOV 

func QuatToOV(q quat.Number) *OrientationVector

QuatToOV converts a quaternion to an orientation vector.

func (*OrientationVector) AxisAngles 

func (ov *OrientationVector) AxisAngles() *R4AA

AxisAngles returns the orientation in axis angle representation.

func (*OrientationVector) Degrees 

func (ov *OrientationVector) Degrees() *OrientationVectorDegrees

Degrees converts the OrientationVector to an OrientationVectorDegrees.

func (*OrientationVector) EulerAngles 

func (ov *OrientationVector) EulerAngles() *EulerAngles

EulerAngles returns orientation in Euler angle representation.

func (*OrientationVector) IsValid 

func (ov *OrientationVector) IsValid() error

IsValid returns an error if configuration is invalid.

func (*OrientationVector) Normalize 

func (ov *OrientationVector) Normalize()

Normalize scales the x, y, and z components of an Orientation Vector to be on the unit sphere.

func (*OrientationVector) OrientationVectorDegrees 

func (ov *OrientationVector) OrientationVectorDegrees() *OrientationVectorDegrees

OrientationVectorDegrees returns orientation as an orientation vector (in degrees).

func (*OrientationVector) OrientationVectorRadians 

func (ov *OrientationVector) OrientationVectorRadians() *OrientationVector

OrientationVectorRadians returns orientation as an orientation vector (in radians).

func (*OrientationVector) Quaternion 

func (ov *OrientationVector) Quaternion() quat.Number

Quaternion returns orientation in quaternion representation.

func (*OrientationVector) RotationMatrix 

func (ov *OrientationVector) RotationMatrix() *RotationMatrix

RotationMatrix returns the orientation in rotation matrix representation.

func (*OrientationVector) ToQuat 

func (ov *OrientationVector) ToQuat() quat.Number

ToQuat converts an orientation vector to a quaternion.

type OrientationVectorDegrees 

type OrientationVectorDegrees struct {
	Theta float64 `json:"th"`
	OX    float64 `json:"x"`
	OY    float64 `json:"y"`
	OZ    float64 `json:"z"`
}

OrientationVectorDegrees is the orientation vector between two objects, but expressed in degrees rather than radians. Because protobuf Pose is in degrees, this is necessary.

func NewOrientationVectorDegrees 

func NewOrientationVectorDegrees() *OrientationVectorDegrees

NewOrientationVectorDegrees Creates a zero-initialized OrientationVectorDegrees.

func QuatToOVD 

func QuatToOVD(q quat.Number) *OrientationVectorDegrees

QuatToOVD converts a quaternion to an orientation vector in degrees.

func (*OrientationVectorDegrees) AxisAngles 

func (ovd *OrientationVectorDegrees) AxisAngles() *R4AA

AxisAngles returns the orientation in axis angle representation.

func (*OrientationVectorDegrees) EulerAngles 

func (ovd *OrientationVectorDegrees) EulerAngles() *EulerAngles

EulerAngles returns orientation in Euler angle representation.

func (*OrientationVectorDegrees) IsValid 

func (ovd *OrientationVectorDegrees) IsValid() error

IsValid returns an error if configuration is invalid.

func (*OrientationVectorDegrees) Normalize 

func (ovd *OrientationVectorDegrees) Normalize()

Normalize scales the x, y, and z components of an Orientation Vector to be on the unit sphere.

func (*OrientationVectorDegrees) OrientationVectorDegrees 

func (ovd *OrientationVectorDegrees) OrientationVectorDegrees() *OrientationVectorDegrees

OrientationVectorDegrees returns orientation as an orientation vector (in degrees).

func (*OrientationVectorDegrees) OrientationVectorRadians 

func (ovd *OrientationVectorDegrees) OrientationVectorRadians() *OrientationVector

OrientationVectorRadians returns orientation as an orientation vector (in radians).

func (*OrientationVectorDegrees) Quaternion 

func (ovd *OrientationVectorDegrees) Quaternion() quat.Number

Quaternion returns orientation in quaternion representation.

func (*OrientationVectorDegrees) Radians 

func (ovd *OrientationVectorDegrees) Radians() *OrientationVector

Radians converts a OrientationVectorDegrees to an OrientationVector.

func (*OrientationVectorDegrees) RotationMatrix 

func (ovd *OrientationVectorDegrees) RotationMatrix() *RotationMatrix

RotationMatrix returns the orientation in rotation matrix representation.

func (*OrientationVectorDegrees) ToQuat 

func (ovd *OrientationVectorDegrees) ToQuat() quat.Number

ToQuat converts an orientation vector in degrees to a quaternion.

type Pose 

type Pose interface {
	Point() r3.Vector
	Orientation() Orientation
}

Pose represents a 6dof pose, position and orientation, with respect to the origin. The Point() method returns the position in (x,y,z) mm coordinates, and the Orientation() method returns an Orientation object, which has methods to parametrize the rotation in multiple different representations.

func Compose 

func Compose(a, b Pose) Pose

Compose treats Poses as functions A(x) and B(x), and produces a new function C(x) = A(B(x)). It converts the poses to dual quaternions and multiplies them together, normalizes the transform and returns a new Pose. Composition does not commute in general, i.e. you cannot guarantee ABx == BAx.

func GeoPoseToPose added in v0.15.0 

func GeoPoseToPose(point, origin *GeoPose) Pose

GeoPoseToPose returns the pose of point with respect to origin.

func Interpolate 

func Interpolate(p1, p2 Pose, by float64) Pose

Interpolate will return a new Pose that has been interpolated the set amount between two poses. Note that position and orientation are interpolated separately, then the two are combined. Note that slerp(q1, q2) != slerp(q2, q1) p1 and p2 are the two poses to interpolate between, by is a float representing the amount to interpolate between them. by == 0 will return p1, by == 1 will return p2, and by == 0.5 will return the pose halfway between them.

func NewPose added in v0.2.11 

func NewPose(p r3.Vector, o Orientation) Pose

NewPose takes in a position and orientation and returns a Pose.

func NewPoseFromDH 

func NewPoseFromDH(a, d, alpha float64) Pose

NewPoseFromDH creates a new pose from denavit hartenberg parameters.

func NewPoseFromOrientation 

func NewPoseFromOrientation(o Orientation) Pose

NewPoseFromOrientation takes in an orientation and returns a Pose. It will have the same position as the frame it is in reference to.

func NewPoseFromPoint 

func NewPoseFromPoint(point r3.Vector) Pose

NewPoseFromPoint takes in a cartesian (x,y,z) and stores it as a vector. It will have the same orientation as the frame it is in reference to.

func NewPoseFromProtobuf 

func NewPoseFromProtobuf(pos *commonpb.Pose) Pose

NewPoseFromProtobuf creates a new pose from a protobuf pose.

func NewZeroPose 

func NewZeroPose() Pose

NewZeroPose returns a pose at (0,0,0) with same orientation as whatever frame it is placed in.

func PoseBetween 

func PoseBetween(a, b Pose) Pose

PoseBetween returns the difference between two dualQuaternions, that is, the dq which if multiplied by one will give the other. Example: if PoseBetween(a, b) = c, then Compose(a, c) = b.

func PoseBetweenInverse added in v0.9.0 

func PoseBetweenInverse(a, b Pose) Pose

PoseBetweenInverse returns an origin pose which when composed with the first parameter, yields the second. Example: if PoseBetweenInverse(a, b) = c, then Compose(c, a) = b PoseBetweenInverse(a, b) is equivalent to Compose(b, PoseInverse(a)).

func PoseDelta 

func PoseDelta(a, b Pose) Pose

PoseDelta returns the difference between two dualQuaternion. Useful for measuring distances, NOT to be used for spatial transformations. We use quaternion/angle axis for this because distances are well-defined.

func PoseInverse 

func PoseInverse(p Pose) Pose

PoseInverse will return the inverse of a pose. So if a given pose p is the pose of A relative to B, PoseInverse(p) will give the pose of B relative to A.

func ProjectOrientationTo2dRotation added in v0.28.0 

func ProjectOrientationTo2dRotation(pose Pose) (Pose, error)

ProjectOrientationTo2dRotation takes a pose whose orientation is not pointing at a pole and collapses its orientation into a single rotation around the Z axis. This is useful for determining things like heading on a map if the component reporting heading is askew.

type Quaternion 

type Quaternion quat.Number

Quaternion is an orientation in quaternion representation.

func (*Quaternion) AxisAngles 

func (q *Quaternion) AxisAngles() *R4AA

AxisAngles returns the orientation in axis angle representation.

func (*Quaternion) EulerAngles 

func (q *Quaternion) EulerAngles() *EulerAngles

EulerAngles returns orientation in Euler angle representation.

func (Quaternion) MarshalJSON 

func (q Quaternion) MarshalJSON() ([]byte, error)

MarshalJSON marshals to W, X, Y, Z json.

func (*Quaternion) OrientationVectorDegrees 

func (q *Quaternion) OrientationVectorDegrees() *OrientationVectorDegrees

OrientationVectorDegrees returns orientation as an orientation vector (in degrees).

func (*Quaternion) OrientationVectorRadians 

func (q *Quaternion) OrientationVectorRadians() *OrientationVector

OrientationVectorRadians returns orientation as an orientation vector (in radians).

func (*Quaternion) Quaternion 

func (q *Quaternion) Quaternion() quat.Number

Quaternion returns orientation in quaternion representation.

func (*Quaternion) RotationMatrix 

func (q *Quaternion) RotationMatrix() *RotationMatrix

RotationMatrix returns the orientation in rotation matrix representation.

type R4AA 

type R4AA struct {
	Theta float64 `json:"th"`
	RX    float64 `json:"x"`
	RY    float64 `json:"y"`
	RZ    float64 `json:"z"`
}

R4AA represents an R4 axis angle.

func NewR4AA 

func NewR4AA() *R4AA

NewR4AA creates an empty R4AA struct.

func QuatToR4AA 

func QuatToR4AA(q quat.Number) *R4AA

QuatToR4AA converts a quat to an R4 axis angle in the same way the C++ Eigen library does. https://eigen.tuxfamily.org/dox/AngleAxis_8h_source.html

func R3ToR4 

func R3ToR4(aa r3.Vector) *R4AA

R3ToR4 converts an R3 angle axis to R4.

func (*R4AA) AxisAngles 

func (r4 *R4AA) AxisAngles() *R4AA

AxisAngles returns the orientation in axis angle representation.

func (*R4AA) EulerAngles 

func (r4 *R4AA) EulerAngles() *EulerAngles

EulerAngles returns orientation in Euler angle representation.

func (*R4AA) Normalize 

func (r4 *R4AA) Normalize()

Normalize scales the x, y, and z components of a R4 axis angle to be on the unit sphere.

func (*R4AA) OrientationVectorDegrees 

func (r4 *R4AA) OrientationVectorDegrees() *OrientationVectorDegrees

OrientationVectorDegrees returns orientation as an orientation vector (in degrees).

func (*R4AA) OrientationVectorRadians 

func (r4 *R4AA) OrientationVectorRadians() *OrientationVector

OrientationVectorRadians returns orientation as an orientation vector (in radians).

func (*R4AA) Quaternion 

func (r4 *R4AA) Quaternion() quat.Number

Quaternion returns orientation in quaternion representation.

func (*R4AA) RotationMatrix 

func (r4 *R4AA) RotationMatrix() *RotationMatrix

RotationMatrix returns the orientation in rotation matrix representation.

func (*R4AA) ToQuat 

func (r4 *R4AA) ToQuat() quat.Number

ToQuat converts an R4 axis angle to a unit quaternion See: https://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToQuaternion/index.htm1

func (*R4AA) ToR3 

func (r4 *R4AA) ToR3() r3.Vector

ToR3 converts an R4 angle axis to R3.

type RotationMatrix 

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

RotationMatrix is a 3x3 matrix in row major order. m[3*r + c] is the element in the r'th row and c'th column.

func MatMul added in v0.1.2 

func MatMul(a, b RotationMatrix) *RotationMatrix

MatMul returns the product of one matrix A applied to B as AB = C

func NewRotationMatrix 

func NewRotationMatrix(m []float64) (*RotationMatrix, error)

NewRotationMatrix creates the rotation matrix from a slice of floats.

func QuatToRotationMatrix 

func QuatToRotationMatrix(q quat.Number) *RotationMatrix

QuatToRotationMatrix converts a quat to a Rotation Matrix reference: https://github.com/go-gl/mathgl/blob/592312d8590acb0686c14740dcf60e2f32d9c618/mgl64/quat.go#L168

func (*RotationMatrix) At 

func (rm *RotationMatrix) At(row, col int) float64

At returns the float corresponding to the element at the specified location.

func (*RotationMatrix) AxisAngles 

func (rm *RotationMatrix) AxisAngles() *R4AA

AxisAngles returns the orientation in axis angle representation.

func (*RotationMatrix) Col 

func (rm *RotationMatrix) Col(col int) r3.Vector

Col returns the a 3 element vector corresponding to the specified col.

func (*RotationMatrix) EulerAngles 

func (rm *RotationMatrix) EulerAngles() *EulerAngles

EulerAngles returns orientation in Euler angle representation.

func (*RotationMatrix) LeftMatMul added in v0.1.2 

func (rm *RotationMatrix) LeftMatMul(lmm RotationMatrix) *RotationMatrix

LeftMatMul returns the right side multiplication a matrix A by matrix B, AB.

func (*RotationMatrix) Mul 

func (rm *RotationMatrix) Mul(v r3.Vector) r3.Vector

Mul returns the product of the rotation Matrix with an r3 Vector.

func (*RotationMatrix) OrientationVectorDegrees 

func (rm *RotationMatrix) OrientationVectorDegrees() *OrientationVectorDegrees

OrientationVectorDegrees returns orientation as an orientation vector (in degrees).

func (*RotationMatrix) OrientationVectorRadians 

func (rm *RotationMatrix) OrientationVectorRadians() *OrientationVector

OrientationVectorRadians returns orientation as an orientation vector (in radians).

func (*RotationMatrix) Quaternion 

func (rm *RotationMatrix) Quaternion() quat.Number

Quaternion returns orientation in quaternion representation. reference: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm

func (*RotationMatrix) RightMatMul added in v0.1.2 

func (rm *RotationMatrix) RightMatMul(rmm RotationMatrix) *RotationMatrix

RightMatMul returns the left side multiplication a matrix A by matrix B, BA

func (*RotationMatrix) RotationMatrix 

func (rm *RotationMatrix) RotationMatrix() *RotationMatrix

RotationMatrix returns the orientation in rotation matrix representation.

func (*RotationMatrix) Row 

func (rm *RotationMatrix) Row(row int) r3.Vector

Row returns the a 3 element vector corresponding to the specified row.

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.