Math Helpers¶
-
class
bosdyn.client.math_helpers.
SE2Pose
(x, y, angle)[source]¶ Bases:
object
Class representing an SE2Pose with position and angle.
-
static
flatten
(se3pose)[source]¶ Flatten a given SE3Pose to an SE2Pose. This will loose height information if the se3pose provided is not gravity aligned. The common gravity aligned frames are odom, vision, and flat_body.
- Inputs:
se3pose (math_helpers.SE3Pose)
- Returns
math_helpers.SE2Pose representing the flattened se3pose.
-
to_obj
(proto)[source]¶ Adds the math_helpers.SE2Pose properties into the geometry_pb2.SE2Pose ‘proto’.
-
to_proto
()[source]¶ Converts the math_helpers.SE2Pose into an output of the protobuf geometry_pb2.SE2Pose.
-
inverse
()[source]¶ Compute the inverse of the math_helpers.SE2Pose.
For example, if the SE(2) pose represented a_tform_b, then the inverse pose is b_tform_a.
- Returns
math_helpers.SE2Pose representing the inverse of the current SE(2) Pose.
-
mult
(se2pose)[source]¶ Computes the multiplication between the current math_helpers.SE2Pose and the input se2pose.
For example, if the ‘self’ SE2Pose represents a_tform_b and the input se2pose represents b_tform_c, then the output will represent the transform a_tform_c.
- Inputs:
se2pose (math_helpers.se2pose)
- Returns
math_helpers.se2pose representing the multiplication of two SE(2) poses.
-
to_rot_matrix
()[source]¶ Returns the rotation matrix generate from the angle of the current SE(2) Pose.
-
to_adjoint_matrix
()[source]¶ This creates the adjoint matrix for the current SE2Pose.
The adjoint matrix can be used to change reference frames for a SE(2) velocity vector. For example, if you have math_helpers.SE2Velocity velocity_in_frame_b, then the adjoint matrix for the math_helpers.SE2Pose (representing a_tform_b) can be used as follows to transform the velocity: velocity_in_frame_a = a_tform_b.to_adjoint_matrix() * velocity_in_frame_b
- Returns
a_adjoint_b (Numpy 3x3 matrix) representing the adjoint matrix for the SE2Pose.
-
property
position
¶ Property to allow attribute access of the protobuf message field ‘position’ similar to the geometry_pb2.SE2Pose for the math_helper.SE2Pose.
-
static
-
class
bosdyn.client.math_helpers.
SE2Velocity
(x, y, angular)[source]¶ Bases:
object
Class representing an SE2Velocity with linear velocity and angular velocity.
-
to_obj
(proto)[source]¶ Adds the math_helpers.SE2Velocity properties into the geometry_pb2.SE2Velocity ‘proto’.
-
to_proto
()[source]¶ Converts the math_helpers.SE2Velocity into an output of the protobuf geometry_pb2.SE2Velocity.
-
static
from_vector
(se2_vel_vector)[source]¶ Converts a 3x1 velocity vector (of either a numpy array or a list) into a math_helpers.SE2Velocity object.
-
property
linear
¶ Property to allow attribute access of the protobuf message field ‘linear’ similar to the geometry_pb2.SE2Velocity for the math_helper.SE2Velocity.
-
property
angular
¶ Property to allow attribute access of the protobuf message field ‘angular’ similar to the geometry_pb2.SE2Velocity for the math_helper.SE2Velocity.
-
-
class
bosdyn.client.math_helpers.
SE3Velocity
(lin_x, lin_y, lin_z, ang_x, ang_y, ang_z)[source]¶ Bases:
object
Class representing an SE3Velocity with linear velocity and angular velocity.
-
to_obj
(proto)[source]¶ Adds the math_helpers.SE3Velocity properties into the geometry_pb2.SE3Velocity ‘proto’.
-
to_proto
()[source]¶ Converts the math_helpers.SE3Velocity into an output of the protobuf geometry_pb2.SE3Velocity.
-
property
linear
¶ Property to allow attribute access of the protobuf message field ‘linear’ similar to the geometry_pb2.SE3Velocity for the math_helper.SE3Velocity.
-
property
angular
¶ Property to allow attribute access of the protobuf message field ‘angular’ similar to the geometry_pb2.SE3Velocity for the math_helper.SE3Velocity.
-
-
class
bosdyn.client.math_helpers.
SE3Pose
(x, y, z, rot)[source]¶ Bases:
object
Class representing an SE3Pose with position and rotation.
-
to_obj
(proto)[source]¶ Adds the math_helpers.SE3Pose properties into the geometry_pb2.SE3Pose ‘proto’.
-
to_proto
()[source]¶ Converts the math_helpers.SE3Pose into an output of the protobuf geometry_pb2.SE3Pose.
-
inverse
()[source]¶ Compute the inverse of the math_helpers.SE3Pose.
For example, if the SE(3) pose represented a_tform_b, then the inverse pose is b_tform_a.
- Returns
math_helpers.SE3Pose representing the inverse of the current SE(3) Pose.
-
transform_point
(x, y, z)[source]¶ Compute the transformation (translation and rotation) of a (x,y,z) vector using the current SE(3) pose.
- Returns
tuple (size 3) representing the transformed coordinate.
-
transform_vec3
(vec3)[source]¶ Compute the transformation (translation and rotation) of a (x,y,z) vector using the current SE(3) pose.
- Returns
Vec3 representing the transformed coordinate.
-
transform_cloud
(points)[source]¶ Compute the transformation (translation and rotation) of multiple vector/points using the current math_helpers.SE3Pose.
- Inputs:
points (Nx3 numpy matrix) representing a set of (x,y,z) points to be transformed
- Returns
Nx3 numpy matrix of the points after they are transformed with the current math_helpers.SE3Pose.
-
static
transform_cloud_from_matrix
(transform, points)[source]¶ Compute the transformation (translation and rotation) of multiple vector/points using the input SE(3) pose.
- Inputs:
transform (4x4 numpy matrix) representing the SE3Pose to be used to transform. points (Nx3 numpy matrix) representing a set of (x,y,z) points to be transformed
- Returns
Nx3 numpy matrix of the points after they are transformed.
-
mult
(se3pose)[source]¶ Computes the multiplication between the current math_helpers.SE3Pose and the input se3pose.
For example, if the ‘self’ SE3Pose represents a_tform_b and the input se3pose represents b_tform_c, then the output will represent the transform a_tform_c.
- Inputs:
se3pose (math_helpers.SE3Pose)
- Returns
math_helpers.SE3Pose representing the multiplication of two SE(3) poses.
-
property
position
¶ Property to allow attribute access of the protobuf message field ‘position’ similar to the geometry_pb2.SE3Pose for the math_helper.SE3Pose.
-
property
rotation
¶ Property to allow attribute access of the protobuf message field ‘rotation’ similar to the geometry_pb2.SE3Pose for the math_helper.SE3Pose.
-
get_translation
()[source]¶ Returns a 3x1 numpy array representing the translation only of the current SE3Pose.
-
to_adjoint_matrix
()[source]¶ This creates the adjoint matrix for the current SE3Pose.
The adjoint matrix can be used to change reference frames for a SE(3) velocity vector. For example, if you have math_helpers.SE3Velocity velocity_in_frame_b, then the adjoint matrix for the math_helpers.SE3Pose (representing a_tform_b) can be used as follows to transform the velocity: velocity_in_frame_a = a_tform_b.to_adjoint_matrix() * velocity_in_frame_b
- Returns
a_adjoint_b (Numpy 6x6 matrix) representing the adjoint matrix for the SE3Pose.
-
-
class
bosdyn.client.math_helpers.
Quat
(w=1, x=0, y=0, z=0)[source]¶ Bases:
object
Class representing a Quaternion.
-
transform_point
(x, y, z)[source]¶ “Computes the transformation (rotation by the quaternion) of a single (x,y,z) point using the current math_helpers.Quat.
-
transform_vec3
(vec3)[source]¶ “Computes the transformation (rotation by the quaternion) of a Vec3 point using the current math_helpers.Quat.
-
static
from_matrix
(rot)[source]¶ Creates a math_helpers.Quat from a numpy 3x3 matrix representing rotation.
-
static
from_roll
(angle)[source]¶ Computes a representative math_helpers.Quat from the Euler angle for roll.
-
static
from_pitch
(angle)[source]¶ Computes a representative math_helpers.Quat from the Euler angle for pitch.
-
static
from_yaw
(angle)[source]¶ Computes a representative math_helpers.Quat from the Euler angle for yaw.
-
to_obj
(proto)[source]¶ Adds the math_helpers.Quat properties into the geometry_pb2.Quaternion ‘proto’.
-
-
bosdyn.client.math_helpers.
pose_to_xyz_yaw
(A_tform_B)[source]¶ Gets the x,y,z yaw of B in A from the SE3Pose protobuf message.
-
bosdyn.client.math_helpers.
is_within_threshold
(pose_3d, max_translation_meters, max_yaw_degrees)[source]¶ Determines whether the given SE3 pose is small enough in X, Y, and theta.
-
bosdyn.client.math_helpers.
skew_matrix_3d
(vec3_proto)[source]¶ Creates a 3x3 numpy matrix representing the skew symmetric matrix for a vec3. These are used to create the adjoint matrices for SE3Pose’s, among other things.
-
bosdyn.client.math_helpers.
skew_matrix_2d
(vec2_proto)[source]¶ Creates a 2x1 numpy matrix representing the skew symmetric matrix for a vec2. These are used to create the adjoint matrices for SE2Pose’s, among other things.
-
bosdyn.client.math_helpers.
transform_se2velocity
(a_adjoint_b_matrix, se2_velocity_in_b)[source]¶ Changes the frame that the SE(2) Velocity is expressed in. More specifically, it converts the SE(2) Velocity in frame b to a SE(2) Velocity in frame c using the adjoint matrix a_adjoint_b.
- Inputs:
a_adjoint_b_matrix (3x3 numpy matrix) se2_velocity_in_b (geometry_pb2.SE2Velocity OR math_helpers.SE2Velocity) described in frame B.
- Returns
math_helpers.SE2Velocity described in frame a. None if the input velocity is an unknown type.
-
bosdyn.client.math_helpers.
transform_se3velocity
(a_adjoint_b_matrix, se3_velocity_in_b)[source]¶ Changes the frame that the SE(3) Velocity is expressed in. More specifically, it converts the SE(3) Velocity in frame b to a SE(3) Velocity in frame c using the adjoint matrix a_adjoint_b.
- Inputs:
a_adjoint_b_matrix (6x6 numpy matrix) se3_velocity_in_b (geometry_pb2.SE3Velocity OR math_helpers.SE3Velocity) described in frame B.
- Returns
math_helpers.SE3Velocity described in frame a. None if the input velocity is an unknown type.