Edit on GitHub

Public Documentation

Documentation for ModernRoboticsBook.jl's public interface.

Contents

Index

Public Interface

ModernRoboticsBook.Adjoint — Method
Adjoint(T)

Computes the adjoint representation of a homogeneous transformation matrix.

Examples

julia> Adjoint([1 0 0 0; 0 0 -1 0; 0 1 0 3; 0 0 0 1])
6×6 Matrix{Float64}:
 1.0  0.0   0.0  0.0  0.0   0.0
 0.0  0.0  -1.0  0.0  0.0   0.0
 0.0  1.0   0.0  0.0  0.0   0.0
 0.0  0.0   3.0  1.0  0.0   0.0
 3.0  0.0   0.0  0.0  0.0  -1.0
 0.0  0.0   0.0  0.0  1.0   0.0
source
ModernRoboticsBook.AxisAng3 — Method
AxisAng3(expc3)

Converts a 3-vector of exponential coordinates for rotation into axis-angle form.

Examples

julia> AxisAng3([1, 2, 3])
([0.2672612419124244, 0.5345224838248488, 0.8017837257372732], 3.7416573867739413)
source
ModernRoboticsBook.AxisAng6 — Method
AxisAng6(expc6)

Converts a 6-vector of exponential coordinates into screw axis-angle form.

Examples

julia> AxisAng6([1, 0, 0, 1, 2, 3])
([1.0, 0.0, 0.0, 1.0, 2.0, 3.0], 1.0)
source
ModernRoboticsBook.CartesianTrajectory — Method
CartesianTrajectory(Xstart, Xend, Tf, N, method)

Computes a trajectory as a list of N SE(3) matrices corresponding to the origin of the end-effector frame following a straight line.

source
ModernRoboticsBook.ComputedTorque — Method
ComputedTorque(thetalist, dthetalist, eint, g, Mlist, Glist, Slist, thetalistd, dthetalistd, ddthetalistd, Kp, Ki, Kd)

Computes the joint control torques at a particular time instant.

source
ModernRoboticsBook.DistanceToSE3 — Method
DistanceToSE3(mat)

Returns the Frobenius norm to describe the distance of mat from the SE(3) manifold.

Examples

julia> DistanceToSE3([1.0 0.0 0.0 1.2; 0.0 0.1 -0.95 1.5; 0.0 1.0 0.1 -0.9; 0.0 0.0 0.1 0.98])
0.13493053768513638
source
ModernRoboticsBook.DistanceToSO3 — Method
DistanceToSO3(mat)

Returns the Frobenius norm to describe the distance of mat from the SO(3) manifold.

Examples

julia> DistanceToSO3([1.0 0.0 0.0; 0.0 0.1 -0.95; 0.0 1.0 0.1])
0.08835298523536149
source
ModernRoboticsBook.EndEffectorForces — Method
EndEffectorForces(thetalist, Ftip, Mlist, Glist, Slist)

Computes the joint forces/torques an open chain robot requires only to create the end-effector force Ftip.

Arguments

  • thetalist: the $n$-vector of joint variables.
  • Ftip: the spatial force applied by the end-effector expressed in frame {n+1}.
  • Mlist: the list of link frames i relative to i-1 at the home position.
  • Glist: the spatial inertia matrices Gi of the links.
  • Slist: the screw axes Si of the joints in a space frame, in the format of a matrix with axes as the columns.

Returns the joint forces and torques required only to create the end-effector force Ftip. This function calls InverseDynamics with g = 0, dthetalist = 0, and ddthetalist = 0.

Examples

julia> import LinearAlgebra as LA

julia> thetalist = [0.1, 0.1, 0.1]
3-element Vector{Float64}:
 0.1
 0.1
 0.1

julia> Ftip = [1, 1, 1, 1, 1, 1]
6-element Vector{Int64}:
 1
 1
 1
 1
 1
 1

julia> M01 = [1 0 0        0;
              0 1 0        0;
              0 0 1 0.089159;
              0 0 0        1]
4×4 Matrix{Float64}:
 1.0  0.0  0.0  0.0
 0.0  1.0  0.0  0.0
 0.0  0.0  1.0  0.089159
 0.0  0.0  0.0  1.0

julia> M12 = [ 0 0 1    0.28;
               0 1 0 0.13585;
              -1 0 0       0;
               0 0 0       1]
4×4 Matrix{Float64}:
  0.0  0.0  1.0  0.28
  0.0  1.0  0.0  0.13585
 -1.0  0.0  0.0  0.0
  0.0  0.0  0.0  1.0

julia> M23 = [1 0 0       0;
              0 1 0 -0.1197;
              0 0 1   0.395;
              0 0 0       1]
4×4 Matrix{Float64}:
 1.0  0.0  0.0   0.0
 0.0  1.0  0.0  -0.1197
 0.0  0.0  1.0   0.395
 0.0  0.0  0.0   1.0

julia> M34 = [1 0 0       0;
              0 1 0       0;
              0 0 1 0.14225;
              0 0 0       1]
4×4 Matrix{Float64}:
 1.0  0.0  0.0  0.0
 0.0  1.0  0.0  0.0
 0.0  0.0  1.0  0.14225
 0.0  0.0  0.0  1.0

julia> Mlist = [M01, M12, M23, M34]
4-element Vector{Matrix{Float64}}:
 [1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0; 0.0 0.0 1.0 0.089159; 0.0 0.0 0.0 1.0]
 [0.0 0.0 1.0 0.28; 0.0 1.0 0.0 0.13585; -1.0 0.0 0.0 0.0; 0.0 0.0 0.0 1.0]
 [1.0 0.0 0.0 0.0; 0.0 1.0 0.0 -0.1197; 0.0 0.0 1.0 0.395; 0.0 0.0 0.0 1.0]
 [1.0 0.0 0.0 0.0; 0.0 1.0 0.0 0.0; 0.0 0.0 1.0 0.14225; 0.0 0.0 0.0 1.0]

julia> G1 = LA.Diagonal([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7])
6×6 LinearAlgebra.Diagonal{Float64, Vector{Float64}}:
 0.010267   ⋅         ⋅        ⋅    ⋅    ⋅ 
  ⋅        0.010267   ⋅        ⋅    ⋅    ⋅ 
  ⋅         ⋅        0.00666   ⋅    ⋅    ⋅ 
  ⋅         ⋅         ⋅       3.7   ⋅    ⋅ 
  ⋅         ⋅         ⋅        ⋅   3.7   ⋅ 
  ⋅         ⋅         ⋅        ⋅    ⋅   3.7

julia> G2 = LA.Diagonal([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393])
6×6 LinearAlgebra.Diagonal{Float64, Vector{Float64}}:
 0.22689   ⋅        ⋅          ⋅      ⋅      ⋅ 
  ⋅       0.22689   ⋅          ⋅      ⋅      ⋅ 
  ⋅        ⋅       0.0151074   ⋅      ⋅      ⋅ 
  ⋅        ⋅        ⋅         8.393   ⋅      ⋅ 
  ⋅        ⋅        ⋅          ⋅     8.393   ⋅ 
  ⋅        ⋅        ⋅          ⋅      ⋅     8.393

julia> G3 = LA.Diagonal([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275])
6×6 LinearAlgebra.Diagonal{Float64, Vector{Float64}}:
 0.0494433   ⋅          ⋅         ⋅      ⋅      ⋅ 
  ⋅         0.0494433   ⋅         ⋅      ⋅      ⋅ 
  ⋅          ⋅         0.004095   ⋅      ⋅      ⋅ 
  ⋅          ⋅          ⋅        2.275   ⋅      ⋅ 
  ⋅          ⋅          ⋅         ⋅     2.275   ⋅ 
  ⋅          ⋅          ⋅         ⋅      ⋅     2.275

julia> Glist = [G1, G2, G3]
3-element Vector{LinearAlgebra.Diagonal{Float64, Vector{Float64}}}:
 [0.010267 0.0 … 0.0 0.0; 0.0 0.010267 … 0.0 0.0; … ; 0.0 0.0 … 3.7 0.0; 0.0 0.0 … 0.0 3.7]
 [0.22689 0.0 … 0.0 0.0; 0.0 0.22689 … 0.0 0.0; … ; 0.0 0.0 … 8.393 0.0; 0.0 0.0 … 0.0 8.393]
 [0.0494433 0.0 … 0.0 0.0; 0.0 0.0494433 … 0.0 0.0; … ; 0.0 0.0 … 2.275 0.0; 0.0 0.0 … 0.0 2.275]

julia> Slist = [ 1  0  1      0  1      0;
                 0  1  0 -0.089  0      0;
                 0  1  0 -0.089  0  0.425]'
6×3 adjoint(::Matrix{Float64}) with eltype Float64:
 1.0   0.0     0.0
 0.0   1.0     1.0
 1.0   0.0     0.0
 0.0  -0.089  -0.089
 1.0   0.0     0.0
 0.0   0.0     0.425

julia> EndEffectorForces(thetalist, Ftip, Mlist, Glist, Slist)
3-element Vector{Float64}:
 1.4095460782639782
 1.857714972318063
 1.392409
source
ModernRoboticsBook.EulerStep — Method
EulerStep(thetalist, dthetalist, ddthetalist, dt)

Compute the joint angles and velocities at the next timestep using first order Euler integration.

Arguments

  • thetalist: the $n$-vector of joint variables.
  • dthetalist: the $n$-vector of joint rates.
  • ddthetalist: the $n$-vector of joint accelerations.
  • dt: the timestep delta t.

Return

  • thetalistNext: the vector of joint variables after dt from first order Euler integration.
  • dthetalistNext: the vector of joint rates after dt from first order Euler integration.

Examples

julia> EulerStep([0.1, 0.1, 0.1], [0.1, 0.2, 0.3], [2, 1.5, 1], 0.1)
([0.11000000000000001, 0.12000000000000001, 0.13], [0.30000000000000004, 0.35000000000000003, 0.4])
source
ModernRoboticsBook.FKinBody — Method
FKinBody(M, Blist, thetalist)

Computes forward kinematics in the body frame for an open chain robot.

Examples

julia> M = [ -1  0  0  0 ;
              0  1  0  6 ;
              0  0 -1  2 ;
              0  0  0  1 ];

julia> Blist = [  0  0 -1  2  0  0   ;
                  0  0  0  0  1  0   ;
                  0  0  1  0  0  0.1 ]';

julia> thetalist = [ π/2, 3, π ];

julia> FKinBody(M, Blist, thetalist)
4×4 Matrix{Float64}:
 -1.14424e-17  1.0           0.0  -5.0    
  1.0          1.14424e-17   0.0   4.0    
  0.0          0.0          -1.0   1.68584
  0.0          0.0           0.0   1.0
source
ModernRoboticsBook.FKinSpace — Method
FKinSpace(M, Slist, thetalist)

Computes forward kinematics in the space frame for an open chain robot.

Examples

julia> M = [ -1  0  0  0 ;
              0  1  0  6 ;
              0  0 -1  2 ;
              0  0  0  1 ];

julia> Slist = [  0  0  1  4  0  0   ;
                  0  0  0  0  1  0   ;
                  0  0 -1 -6  0 -0.1 ]';

julia> thetalist = [ π/2, 3, π ];

julia> FKinSpace(M, Slist, thetalist)
4×4 Matrix{Float64}:
 -1.14424e-17  1.0           0.0  -5.0    
  1.0          1.14424e-17   0.0   4.0    
  0.0          0.0          -1.0   1.68584
  0.0          0.0           0.0   1.0
source
ModernRoboticsBook.ForwardDynamicsTrajectory — Method
ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, Ftipmat, Mlist, Glist, Slist, dt, intRes)

Simulates the motion of a serial chain given an open-loop history of joint forces/torques.

source
ModernRoboticsBook.GravityForces — Method
GravityForces(thetalist, g, Mlist, Glist, Slist)

Computes the joint forces/torques an open chain robot requires to overcome gravity at its configuration.

source
ModernRoboticsBook.IKinBody — Method
IKinBody(Blist, M, T, thetalist0, eomg, ev)

Computes inverse kinematics in the body frame for an open chain robot.

Examples

julia> Blist = [  0  0 -1  2  0  0   ;
                  0  0  0  0  1  0   ;
                  0  0  1  0  0  0.1 ]';

julia> M = [ -1  0  0  0 ;
              0  1  0  6 ;
              0  0 -1  2 ;
              0  0  0  1 ];

julia> T = [ 0  1  0     -5 ;
             1  0  0      4 ;
             0  0 -1 1.6858 ;
             0  0  0      1 ];

julia> thetalist0 = [1.5, 2.5, 3];

julia> eomg, ev = 0.01, 0.001;

julia> IKinBody(Blist, M, T, thetalist0, eomg, ev)
([1.5707381937148923, 2.999666997382942, 3.141539129217613], true)
source
ModernRoboticsBook.IKinSpace — Method
IKinSpace(Slist, M, T, thetalist0, eomg, ev)

Computes inverse kinematics in the space frame for an open chain robot.

Examples

julia> Slist = [  0  0  1  4  0  0   ;
                  0  0  0  0  1  0   ;
                  0  0 -1 -6  0 -0.1 ]';

julia> M = [ -1  0  0  0 ;
              0  1  0  6 ;
              0  0 -1  2 ;
              0  0  0  1 ];

julia> T = [ 0  1  0     -5 ;
             1  0  0      4 ;
             0  0 -1 1.6858 ;
             0  0  0      1 ];

julia> thetalist0 = [1.5, 2.5, 3];

julia> eomg, ev = 0.01, 0.001;

julia> IKinSpace(Slist, M, T, thetalist0, eomg, ev)
([1.5707378296567203, 2.999663844672524, 3.141534199856583], true)
source
ModernRoboticsBook.InverseDynamics — Method
InverseDynamics(thetalist, dthetalist, ddthetalist, g, Ftip, Mlist, Glist, Slist)

Computes inverse dynamics in the space frame for an open chain robot.

source
ModernRoboticsBook.InverseDynamicsTrajectory — Method
InverseDynamicsTrajectory(thetamat, dthetamat, ddthetamat, g, Ftipmat, Mlist, Glist, Slist)

Calculates the joint forces/torques required to move the serial chain along the given trajectory using inverse dynamics.

source
ModernRoboticsBook.JacobianBody — Method
JacobianBody(Blist, thetalist)

Computes the body Jacobian for an open chain robot.

Examples

julia> Blist = [0 0 1   0 0.2 0.2;
                1 0 0   2   0   3;
                0 1 0   0   2   1;
                1 0 0 0.2 0.3 0.4]';

julia> thetalist = [0.2, 1.1, 0.1, 1.2];

julia> JacobianBody(Blist, thetalist)
6×4 Matrix{Float64}:
 -0.0452841  0.995004    0.0       1.0
  0.743593   0.0930486   0.362358  0.0
 -0.667097   0.0361754  -0.932039  0.0
  2.32586    1.66809     0.564108  0.2
 -1.44321    2.94561     1.43307   0.3
 -2.0664     1.82882    -1.58869   0.4
source
ModernRoboticsBook.JacobianSpace — Method
JacobianSpace(Slist, thetalist)

Computes the space Jacobian for an open chain robot.

Examples

julia> Slist = [0 0 1   0 0.2 0.2;
                1 0 0   2   0   3;
                0 1 0   0   2   1;
                1 0 0 0.2 0.3 0.4]';

julia> thetalist = [0.2, 1.1, 0.1, 1.2];

julia> JacobianSpace(Slist, thetalist)
6×4 Matrix{Float64}:
 0.0  0.980067  -0.0901156   0.957494 
 0.0  0.198669   0.444554    0.284876 
 1.0  0.0        0.891207   -0.0452841
 0.0  1.95219   -2.21635    -0.511615 
 0.2  0.436541  -2.43713     2.77536  
 0.2  2.96027    3.23573     2.22512
source
ModernRoboticsBook.MatrixExp3 — Method
MatrixExp3(so3mat)

Computes the matrix exponential of a matrix in so(3).

Examples

julia> MatrixExp3([0 -3 2; 3 0 -1; -2 1 0])
3×3 Matrix{Float64}:
 -0.694921   0.713521  0.0892929
 -0.192007  -0.303785  0.933192 
  0.692978   0.63135   0.348107
source
ModernRoboticsBook.MatrixExp6 — Method
MatrixExp6(se3mat)

Computes the matrix exponential of an se3 representation of exponential coordinates.

Examples

julia> MatrixExp6([0 0 0 0; 0 0 -1.57079632 2.35619449; 0 1.57079632 0 2.35619449; 0 0 0 0])
4×4 Matrix{Float64}:
 1.0  0.0         0.0        0.0       
 0.0  6.7949e-9  -1.0        1.01923e-8
 0.0  1.0         6.7949e-9  3.0       
 0.0  0.0         0.0        1.0
source
ModernRoboticsBook.MatrixLog3 — Method
MatrixLog3(R)

Computes the matrix logarithm of a rotation matrix.

Examples

julia> MatrixLog3([0 0 1; 1 0 0; 0 1 0])
3×3 Matrix{Float64}:
  0.0     -1.2092   1.2092
  1.2092   0.0     -1.2092
 -1.2092   1.2092   0.0
source
ModernRoboticsBook.MatrixLog6 — Method
MatrixLog6(T)

Computes the matrix logarithm of a homogeneous transformation matrix.

Examples

julia> MatrixLog6([1 0 0 0; 0 0 -1 0; 0 1 0 3; 0 0 0 1])
4×4 Matrix{Float64}:
 0.0  0.0      0.0     0.0    
 0.0  0.0     -1.5708  2.35619
 0.0  1.5708   0.0     2.35619
 0.0  0.0      0.0     0.0
source
ModernRoboticsBook.Normalize — Method
Normalize(V)

Normalizes a vector.

Examples

julia> Normalize([1, 2, 3])
3-element Vector{Float64}:
 0.2672612419124244
 0.5345224838248488
 0.8017837257372732
source
ModernRoboticsBook.ProjectToSE3 — Method
ProjectToSE3(mat)

Returns a projection of mat into SE(3).

Examples

julia> ProjectToSE3([0.675 0.150 0.720 1.2; 0.370 0.771 -0.511 5.4; -0.630 0.619 0.472 3.6; 0.003 0.002 0.010 0.9])
4×4 Matrix{Float64}:
  0.679011  0.148945   0.718859  1.2
  0.373207  0.773196  -0.512723  5.4
 -0.632187  0.616428   0.469421  3.6
  0.0       0.0        0.0       1.0
source
ModernRoboticsBook.ProjectToSO3 — Method
ProjectToSO3(mat)

Returns a projection of mat into SO(3).

Examples

julia> ProjectToSO3([0.675 0.150  0.720; 0.370 0.771 -0.511; -0.630 0.619  0.472])
3×3 Matrix{Float64}:
  0.679011  0.148945   0.718859
  0.373207  0.773196  -0.512723
 -0.632187  0.616428   0.469421
source
ModernRoboticsBook.RotInv — Method
RotInv(R)

Inverts a rotation matrix.

Examples

julia> RotInv([0 0 1; 1 0 0; 0 1 0])
3×3 adjoint(::Matrix{Int64}) with eltype Int64:
 0  1  0
 0  0  1
 1  0  0
source
ModernRoboticsBook.RpToTrans — Method
RpToTrans(R, p)

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

Examples

julia> RpToTrans([1 0 0; 0 0 -1; 0 1 0], [1, 2, 5])
4×4 Matrix{Int64}:
 1  0   0  1
 0  0  -1  2
 0  1   0  5
 0  0   0  1
source
ModernRoboticsBook.ScrewToAxis — Method
ScrewToAxis(q, s, h)

Takes a parametric description of a screw axis and converts it to a normalized screw axis.

Examples

julia> ScrewToAxis([3; 0; 0], [0; 0; 1], 2)
6-element Vector{Int64}:
  0
  0
  1
  0
 -3
  2
source
ModernRoboticsBook.ScrewTrajectory — Method
ScrewTrajectory(Xstart, Xend, Tf, N, method)

Computes a trajectory as a list of N SE(3) matrices corresponding to the screw motion about a space screw axis.

source
ModernRoboticsBook.SimulateControl — Method
SimulateControl(thetalist, dthetalist, g, Ftipmat, Mlist, Glist,
                Slist, thetamatd, dthetamatd, ddthetamatd, gtilde,
                Mtildelist, Gtildelist, Kp, Ki, Kd, dt, intRes)

Simulates the computed torque controller over a given desired trajectory.

source
ModernRoboticsBook.TestIfSE3 — Method
TestIfSE3(mat)

Returns true if mat is close to or on the manifold SE(3).

Examples

julia> TestIfSE3([1.0 0.0 0.0 1.2; 0.0 0.1 -0.95 1.5; 0.0 1.0 0.1 -0.9; 0.0 0.0 0.1 0.98])
false
source
ModernRoboticsBook.TestIfSO3 — Method
TestIfSO3(mat)

Returns true if mat is close to or on the manifold SO(3).

Examples

julia> TestIfSO3([1.0 0.0 0.0; 0.0 0.1 -0.95; 0.0 1.0 0.1])
false
source
ModernRoboticsBook.TransInv — Method
TransInv(T)

Inverts a homogeneous transformation matrix.

Examples

julia> TransInv([1 0 0 0; 0 0 -1 0; 0 1 0 3; 0 0 0 1])
4×4 Matrix{Int64}:
 1   0  0   0
 0   0  1  -3
 0  -1  0   0
 0   0  0   1
source
ModernRoboticsBook.TransToRp — Method
TransToRp(T)

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

Examples

julia> TransToRp([1 0 0 0; 0 0 -1 0; 0 1 0 3; 0 0 0 1])
([1 0 0; 0 0 -1; 0 1 0], [0, 0, 3])
source
ModernRoboticsBook.VecTose3 — Method
VecTose3(V)

Converts a spatial velocity vector into a 4x4 matrix in se3.

Examples

julia> VecTose3([1 2 3 4 5 6])
4×4 Matrix{Float64}:
  0.0  -3.0   2.0  4.0
  3.0   0.0  -1.0  5.0
 -2.0   1.0   0.0  6.0
  0.0   0.0   0.0  0.0
source
ModernRoboticsBook.VecToso3 — Method
VecToso3(ω)

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

Examples

julia> VecToso3([1 2 3])
3×3 Matrix{Int64}:
  0  -3   2
  3   0  -1
 -2   1   0
source
ModernRoboticsBook.ad — Method
ad(V)

Calculate the 6x6 matrix [adV] of the given 6-vector.

Examples

julia> ad([1, 2, 3, 4, 5, 6])
6×6 Matrix{Float64}:
  0.0  -3.0   2.0   0.0   0.0   0.0
  3.0   0.0  -1.0   0.0   0.0   0.0
 -2.0   1.0   0.0   0.0   0.0   0.0
  0.0  -6.0   5.0   0.0  -3.0   2.0
  6.0   0.0  -4.0   3.0   0.0  -1.0
 -5.0   4.0   0.0  -2.0   1.0   0.0
source
ModernRoboticsBook.se3ToVec — Method
se3ToVec(se3mat)

Converts an se3 matrix into a spatial velocity vector.

Examples

julia> se3ToVec([0 -3 2 4; 3 0 -1 5; -2 1 0 6; 0 0 0 0])
6-element Vector{Int64}:
 1
 2
 3
 4
 5
 6
source
ModernRoboticsBook.so3ToVec — Method
so3ToVec(so3mat)

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

Examples

julia> so3ToVec([0 -3 2; 3 0 -1; -2 1 0])
3-element Vector{Int64}:
 1
 2
 3
source