spotmicro.Kinematics package

Submodules

spotmicro.Kinematics.LegKinematics module

class spotmicro.Kinematics.LegKinematics.LegIK(legtype='RIGHT', shoulder_length=0.04, elbow_length=0.1, wrist_length=0.125, hip_lim=[-0.548, 0.548], shoulder_lim=[-2.17, 0.97], leg_lim=[-0.1, 2.59])

Bases: object

LeftIK(x, y, z, D)

Left Leg Inverse Kinematics Solver

Parameters:

• x,y,z – hip-to-foot distances in each dimension
• D – leg domain

Returns:

Joint Angles required for desired position

RightIK(x, y, z, D)

Right Leg Inverse Kinematics Solver

Parameters:

• x,y,z – hip-to-foot distances in each dimension
• D – leg domain

Returns:

Joint Angles required for desired position

get_domain(x, y, z)

Calculates the leg’s Domain and caps it in case of a breach

Parameters:x,y,z – hip-to-foot distances in each dimension Returns:Leg Domain D

solve(xyz_coord)

Generic Leg Inverse Kinematics Solver

Parameters:xyz_coord – hip-to-foot distances in each dimension Returns:Joint Angles required for desired position

spotmicro.Kinematics.LieAlgebra module

spotmicro.Kinematics.LieAlgebra.Adjoint(T)

Computes the adjoint representation of a homogeneous transformation matrix

Parameters:T – A homogeneous transformation matrix Returns:The 6x6 adjoint representation [AdT] of T

Example Input:

T = np.array([[1, 0, 0, 0],

[0, 0, -1, 0], [0, 1, 0, 3], [0, 0, 0, 1]])

Output:

np.array([[1, 0, 0, 0, 0, 0],

[0, 0, -1, 0, 0, 0], [0, 1, 0, 0, 0, 0], [0, 0, 3, 1, 0, 0], [3, 0, 0, 0, 0, -1], [0, 0, 0, 0, 1, 0]])

spotmicro.Kinematics.LieAlgebra.RPY(roll, pitch, yaw)

Creates a Roll, Pitch, Yaw Transformation Matrix

Parameters:

• roll – roll component of matrix
• pitch – pitch component of matrix
• yaw – yaw component of matrix

Returns:

The transformation matrix

Example Input:

roll = 0.0 pitch = 0.0 yaw = 0.0

Output:

np.array([[1, 0, 0, 0],

[0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])

spotmicro.Kinematics.LieAlgebra.RotateTranslate(rotation, position)

Creates a Transformation Matrix from a Rotation, THEN, a Translation

Parameters:

• rotation – pure rotation matrix
• translation – pure translation matrix

Returns:

The transformation matrix

spotmicro.Kinematics.LieAlgebra.RpToTrans(R, p)

Converts a rotation matrix and a position vector into homogeneous transformation matrix

Parameters:

• R – A 3x3 rotation matrix
• p – A 3-vector

Returns:

A homogeneous transformation matrix corresponding to the inputs

Example Input:

R = np.array([[1, 0, 0],

[0, 0, -1], [0, 1, 0]])

p = np.array([1, 2, 5])

Output:

np.array([[1, 0, 0, 1],

[0, 0, -1, 2], [0, 1, 0, 5], [0, 0, 0, 1]])

spotmicro.Kinematics.LieAlgebra.TransInv(T)

Inverts a homogeneous transformation matrix

Parameters:T – A homogeneous transformation matrix Returns:The inverse of T

Uses the structure of transformation matrices to avoid taking a matrix inverse, for efficiency.

Example input:

T = np.array([[1, 0, 0, 0],

[0, 0, -1, 0], [0, 1, 0, 3], [0, 0, 0, 1]])

Output:

np.array([[1, 0, 0, 0],

[0, 0, 1, -3], [0, -1, 0, 0], [0, 0, 0, 1]])

spotmicro.Kinematics.LieAlgebra.TransToRp(T)

Converts a homogeneous transformation matrix into a rotation matrix and position vector

Parameters:T – A homogeneous transformation matrix Return R:The corresponding rotation matrix, Return p:The corresponding position vector.

Example Input:

T = np.array([[1, 0, 0, 0],

[0, 0, -1, 0], [0, 1, 0, 3], [0, 0, 0, 1]])

Output:

(np.array([[1, 0, 0],

[0, 0, -1], [0, 1, 0]]),

np.array([0, 0, 3]))

spotmicro.Kinematics.LieAlgebra.TransformVector(xyz_coord, rotation, translation)

Transforms a vector by a specified Rotation THEN Translation Matrix

Parameters:

• xyz_coord – the vector to transform
• rotation – pure rotation matrix
• translation – pure translation matrix

Returns:

The transformed vector

spotmicro.Kinematics.LieAlgebra.VecToso3(omg)

Converts a 3-vector to an so(3) representation

Parameters:omg – A 3-vector Returns:The skew symmetric representation of omg

Example Input:

omg = np.array([1, 2, 3])

Output:

np.array([[ 0, -3, 2],

[ 3, 0, -1], [-2, 1, 0]])

spotmicro.Kinematics.SpotKinematics module

class spotmicro.Kinematics.SpotKinematics.SpotModel(shoulder_length=0.055, elbow_length=0.10652, wrist_length=0.145, hip_x=0.23, hip_y=0.075, foot_x=0.23, foot_y=0.185, height=0.2, com_offset=0.016, shoulder_lim=[-0.548, 0.548], elbow_lim=[-2.17, 0.97], wrist_lim=[-0.1, 2.59])

Bases: object

HipToFoot(orn, pos, T_bf)

Converts a desired position and orientation wrt Spot’s home position, with a desired body-to-foot Transform into a body-to-hip Transform, which is used to extract and return the Hip To Foot Vector.

Parameters:

• orn – A 3x1 np.array([]) with Spot’s Roll, Pitch, Yaw angles
• pos – A 3x1 np.array([]) with Spot’s X, Y, Z coordinates
• T_bf – Dictionary of desired body-to-foot Transforms.

Returns:

Hip To Foot Vector for each of Spot’s Legs.

IK(orn, pos, T_bf)

Uses HipToFoot() to convert a desired position and orientation wrt Spot’s home position into a Hip To Foot Vector, which is fed into the LegIK solver.

Finally, the resultant joint angles are returned from the LegIK solver for each leg.

Parameters:

• orn – A 3x1 np.array([]) with Spot’s Roll, Pitch, Yaw angles
• pos – A 3x1 np.array([]) with Spot’s X, Y, Z coordinates
• T_bf – Dictionary of desired body-to-foot Transforms.

Returns:

Joint angles for each of Spot’s joints.

Module contents