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
- func ClosestPointSegmentPoint(pt1, pt2, query r3.Vector) r3.Vector
- func ClosestPointsSegmentSegment(ap1, ap2, bp1, bp2 r3.Vector) (r3.Vector, r3.Vector)
- func DistToLineSegment(pt1, pt2, query r3.Vector) float64
- func Flip(q quat.Number) quat.Number
- func GeoGeometryToProtobuf(geoObst *GeoGeometry) *commonpb.GeoGeometry
- func GeoPointToPoint(point, origin *geo.Point) r3.Vector
- func GeometriesAlmostEqual(a, b Geometry) bool
- func GetCartesianDistance(p, q *geo.Point) (float64, float64)
- func NewGeometriesToProto(geometries []Geometry) []*commonpb.Geometry
- func Norm(q quat.Number) float64
- func Normalize(q quat.Number) quat.Number
- func OffsetBy(a, b *commonpb.Pose) *commonpb.Pose
- func OrientationAlmostEqual(o1, o2 Orientation) bool
- func OrientationAlmostEqualEps(o1, o2 Orientation, epsilon float64) bool
- func PlaneNormal(p0, p1, p2 r3.Vector) r3.Vector
- func PoseAlmostCoincident(a, b Pose) bool
- func PoseAlmostCoincidentEps(a, b Pose, epsilon float64) bool
- func PoseAlmostEqual(a, b Pose) bool
- func PoseAlmostEqualEps(a, b Pose, epsilon float64) bool
- func PoseMap(p Pose) (map[string]interface{}, error)
- func PoseToProtobuf(p Pose) *commonpb.Pose
- func QuatToR3AA(q quat.Number) r3.Vector
- func QuaternionAlmostEqual(a, b quat.Number, tol float64) bool
- func R3VectorAlmostEqual(a, b r3.Vector, epsilon float64) bool
- func ResetPoseDQTranslation(p Pose, v r3.Vector)
- func SegmentDistanceToSegment(ap1, ap2, bp1, bp2 r3.Vector) float64
- type AngularVelocity
- type AxisConfig
- type EulerAngles
- func (ea *EulerAngles) AxisAngles() *R4AA
- func (ea *EulerAngles) EulerAngles() *EulerAngles
- func (ea *EulerAngles) OrientationVectorDegrees() *OrientationVectorDegrees
- func (ea *EulerAngles) OrientationVectorRadians() *OrientationVector
- func (ea *EulerAngles) Quaternion() quat.Number
- func (ea *EulerAngles) RotationMatrix() *RotationMatrix
- type GeoGeometry
- func GeoGeometriesFromConfig(config *GeoGeometryConfig) ([]*GeoGeometry, error)
- func GeoGeometriesFromConfigs(configs []*GeoGeometryConfig) ([]*GeoGeometry, error)
- func GeoGeometryFromProtobuf(protoGeoObst *commonpb.GeoGeometry) (*GeoGeometry, error)
- func NewGeoGeometry(loc *geo.Point, geom []Geometry) *GeoGeometry
- type GeoGeometryConfig
- type GeoPose
- type Geometry
- func BoundingSphere(geometry Geometry) (Geometry, error)
- func GeoGeometriesToGeometries(obstacles []*GeoGeometry, origin *geo.Point) []Geometry
- func NewBox(pose Pose, dims r3.Vector, label string) (Geometry, error)
- func NewCapsule(offset Pose, radius, length float64, label string) (Geometry, error)
- func NewGeometriesFromProto(proto []*commonpb.Geometry) ([]Geometry, error)
- func NewGeometryFromProto(geometry *commonpb.Geometry) (Geometry, error)
- func NewPoint(pt r3.Vector, label string) Geometry
- func NewSphere(offset Pose, radius float64, label string) (Geometry, error)
- type GeometryConfig
- type GeometryType
- type Orientation
- type OrientationConfig
- type OrientationType
- type OrientationVector
- func (ov *OrientationVector) AxisAngles() *R4AA
- func (ov *OrientationVector) Degrees() *OrientationVectorDegrees
- func (ov *OrientationVector) EulerAngles() *EulerAngles
- func (ov *OrientationVector) IsValid() error
- func (ov *OrientationVector) Normalize()
- func (ov *OrientationVector) OrientationVectorDegrees() *OrientationVectorDegrees
- func (ov *OrientationVector) OrientationVectorRadians() *OrientationVector
- func (ov *OrientationVector) Quaternion() quat.Number
- func (ov *OrientationVector) RotationMatrix() *RotationMatrix
- func (ov *OrientationVector) ToQuat() quat.Number
- type OrientationVectorDegrees
- func (ovd *OrientationVectorDegrees) AxisAngles() *R4AA
- func (ovd *OrientationVectorDegrees) EulerAngles() *EulerAngles
- func (ovd *OrientationVectorDegrees) IsValid() error
- func (ovd *OrientationVectorDegrees) Normalize()
- func (ovd *OrientationVectorDegrees) OrientationVectorDegrees() *OrientationVectorDegrees
- func (ovd *OrientationVectorDegrees) OrientationVectorRadians() *OrientationVector
- func (ovd *OrientationVectorDegrees) Quaternion() quat.Number
- func (ovd *OrientationVectorDegrees) Radians() *OrientationVector
- func (ovd *OrientationVectorDegrees) RotationMatrix() *RotationMatrix
- func (ovd *OrientationVectorDegrees) ToQuat() quat.Number
- type Pose
- func Compose(a, b Pose) Pose
- func GeoPoseToPose(point, origin *GeoPose) Pose
- func Interpolate(p1, p2 Pose, by float64) Pose
- func NewPose(p r3.Vector, o Orientation) Pose
- func NewPoseFromDH(a, d, alpha float64) Pose
- func NewPoseFromOrientation(o Orientation) Pose
- func NewPoseFromPoint(point r3.Vector) Pose
- func NewPoseFromProtobuf(pos *commonpb.Pose) Pose
- func NewZeroPose() Pose
- func PoseBetween(a, b Pose) Pose
- func PoseBetweenInverse(a, b Pose) Pose
- func PoseDelta(a, b Pose) Pose
- func PoseInverse(p Pose) Pose
- func ProjectOrientationTo2dRotation(pose Pose) (Pose, error)
- type Quaternion
- func (q *Quaternion) AxisAngles() *R4AA
- func (q *Quaternion) EulerAngles() *EulerAngles
- func (q Quaternion) MarshalJSON() ([]byte, error)
- func (q *Quaternion) OrientationVectorDegrees() *OrientationVectorDegrees
- func (q *Quaternion) OrientationVectorRadians() *OrientationVector
- func (q *Quaternion) Quaternion() quat.Number
- func (q *Quaternion) RotationMatrix() *RotationMatrix
- type R4AA
- func (r4 *R4AA) AxisAngles() *R4AA
- func (r4 *R4AA) EulerAngles() *EulerAngles
- func (r4 *R4AA) Normalize()
- func (r4 *R4AA) OrientationVectorDegrees() *OrientationVectorDegrees
- func (r4 *R4AA) OrientationVectorRadians() *OrientationVector
- func (r4 *R4AA) Quaternion() quat.Number
- func (r4 *R4AA) RotationMatrix() *RotationMatrix
- func (r4 *R4AA) ToQuat() quat.Number
- func (r4 *R4AA) ToR3() r3.Vector
- type RotationMatrix
- func (rm *RotationMatrix) At(row, col int) float64
- func (rm *RotationMatrix) AxisAngles() *R4AA
- func (rm *RotationMatrix) Col(col int) r3.Vector
- func (rm *RotationMatrix) EulerAngles() *EulerAngles
- func (rm *RotationMatrix) LeftMatMul(lmm RotationMatrix) *RotationMatrix
- func (rm *RotationMatrix) Mul(v r3.Vector) r3.Vector
- func (rm *RotationMatrix) OrientationVectorDegrees() *OrientationVectorDegrees
- func (rm *RotationMatrix) OrientationVectorRadians() *OrientationVector
- func (rm *RotationMatrix) Quaternion() quat.Number
- func (rm *RotationMatrix) RightMatMul(rmm RotationMatrix) *RotationMatrix
- func (rm *RotationMatrix) RotationMatrix() *RotationMatrix
- func (rm *RotationMatrix) Row(row int) r3.Vector
Constants ¶
const ( UnknownType = GeometryType("") BoxType = GeometryType("box") SphereType = GeometryType("sphere") CapsuleType = GeometryType("capsule") PointType = GeometryType("point") )
The set of allowed representations for orientation.
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
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
ClosestPointsSegmentSegment will return the points at which two line segments are closest to one another.
func DistToLineSegment ¶ added in v0.2.13
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 ¶
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
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
GeometriesAlmostEqual returns a bool describing if the two input Geometries are equal.
func GetCartesianDistance ¶ added in v0.3.0
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
NewGeometriesToProto converts a list of Geometries to profobuf.
func Norm ¶
Norm returns the norm of the quaternion, i.e. the sqrt of the sum of the squares of the imaginary parts.
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
PlaneNormal returns the plane normal of the triangle defined by the three given points.
func PoseAlmostCoincident ¶
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 ¶
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 ¶
PoseAlmostEqual will return a bool describing whether 2 poses are approximately the same.
func PoseAlmostEqualEps ¶ added in v0.2.1
PoseAlmostEqualEps will return a bool describing whether 2 poses are approximately the same.
func PoseMap ¶
PoseMap encodes the orientation interface to something serializable and human readable.
func PoseToProtobuf ¶
PoseToProtobuf converts a pose to the pose format protobuf expects (which is as OrientationVectorDegrees).
func QuatToR3AA ¶
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 ¶
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 ¶
R3VectorAlmostEqual compares two r3.Vector objects and returns if the all elementwise differences are less than epsilon.
func ResetPoseDQTranslation ¶ added in v0.1.2
ResetPoseDQTranslation takes a Pose that must be a dualQuaternion and reset's it's translation.
func SegmentDistanceToSegment ¶ added in v0.2.13
SegmentDistanceToSegment will compute the distance separating two line segments at their closest point.
Types ¶
type AngularVelocity ¶
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 ¶
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
NewGeoPose constructs a GeoPose from a geo.Point and float64.
func PoseToGeoPose ¶ added in v0.15.0
PoseToGeoPose converts a pose (which are always in mm) into a GeoPose treating relativeTo as the origin.
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
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 NewCapsule ¶ added in v0.2.13
NewCapsule instantiates a new capsule Geometry.
func NewGeometriesFromProto ¶ added in v0.3.0
NewGeometriesFromProto converts a list of Geometries from protobuf.
func NewGeometryFromProto ¶
NewGeometryFromProto instantiates a new Geometry from a protobuf Geometry message.
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 ¶
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
GeoPoseToPose returns the pose of point with respect to origin.
func Interpolate ¶
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 ¶
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 ¶
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 ¶
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 ¶
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
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 ¶
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 ¶
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
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 ¶
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 QuatToR4AA ¶
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 (*R4AA) AxisAngles ¶
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 ¶
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 ¶
ToQuat converts an R4 axis angle to a unit quaternion See: https://www.euclideanspace.com/maths/geometry/rotations/conversions/angleToQuaternion/index.htm1
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.