Public Documentation

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

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)

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)

CartesianTrajectory(transform_start, transform_end, total_time, N, method)

Computes a trajectory as a list of $N$ SE(3) matrices where the origin of the end-effector frame follows a straight line and the rotation follows a geodesic on SO(3). Unlike ScrewTrajectory, the position is decoupled from the rotation.

Arguments

• transform_start: the initial end-effector configuration (4×4 SE(3) matrix).
• transform_end: the final end-effector configuration (4×4 SE(3) matrix).
• total_time: total time of the motion in seconds from rest to rest.
• N: the number of points $N \geq 2$ in the discrete representation of the trajectory.
• method: the time-scaling method — use 3 for cubic (CubicTimeScaling) or 5 for quintic (QuinticTimeScaling).

Returns

A list of $N$ SE(3) matrices separated in time by $T_f / (N - 1)$. The first in the list is transform_start and the $N$th is transform_end.

Examples

julia> Xstart = [1 0 0 1; 0 1 0 0; 0 0 1 1; 0 0 0 1];

julia> Xend = [0 0 1 0.1; 1 0 0 0; 0 1 0 4.1; 0 0 0 1];

julia> traj = CartesianTrajectory(Xstart, Xend, 5, 4, 5);

julia> length(traj)
4

julia> traj[1]
4×4 Matrix{Float64}:
 1.0  0.0  0.0  1.0
 0.0  1.0  0.0  0.0
 0.0  0.0  1.0  1.0
 0.0  0.0  0.0  1.0

ComputedTorque(joint_positions, joint_velocities, error_integral, gravity, link_frames, spatial_inertias, screw_axes, desired_joint_positions, desired_joint_velocities, desired_joint_accelerations, Kp, Ki, Kd)

Computes the joint control torques at a particular time instant using the computed torque control law:

$\tau = \widehat{M}(\theta)(\ddot{\theta}_d + K_p e + K_i \int e + K_d \dot{e}) + \widehat{h}(\theta, \dot{\theta})$

where $e = \theta_d - \theta$, $\dot{e} = \dot{\theta}_d - \dot{\theta}$, $\widehat{M}$ is the model of the robot's mass matrix, and $\widehat{h}$ is the model of centripetal, Coriolis, and gravitational forces.

Arguments

• joint_positions: the $n$-vector of current joint variables $\theta$.
• joint_velocities: the $n$-vector of current joint rates $\dot{\theta}$.
• error_integral: the $n$-vector of the time-integral of joint errors $\int e \, dt$.
• gravity: the gravity vector $g$ (e.g., [0, 0, -9.8]).
• link_frames: a vector of $n+1$ SE(3) matrices, where link_frames[i] is $M_{i-1,i}$ and link_frames[n+1] is $M_{n,n+1}$.
• spatial_inertias: a vector of $n$ symmetric 6×6 spatial inertia matrices $G_i$ of the links.
• screw_axes: the screw axes $S_i$ of the joints in a space frame, as a 6×$n$ matrix with axes as columns.
• desired_joint_positions: the $n$-vector of desired joint variables $\theta_d$.
• desired_joint_velocities: the $n$-vector of desired joint rates $\dot{\theta}_d$.
• desired_joint_accelerations: the $n$-vector of desired joint accelerations $\ddot{\theta}_d$.
• Kp: the proportional gain (scalar, applied as $K_p I$).
• Ki: the integral gain (scalar).
• Kd: the derivative gain (scalar).

Returns

The $n$-vector of joint control torques.

Examples

julia> ComputedTorque([0.1, 0.1, 0.1], [0.1, 0.2, 0.3], [0.2, 0.2, 0.2], [0, 0, -9.8], link_frames, spatial_inertias, screw_axes, [1.0, 1.0, 1.0], [2.0, 1.2, 2.0], [0.1, 0.1, 0.1], 1.3, 1.2, 1.1)
3-element Vector{Float64}:
 133.0052524649953
 -29.942233243760633
  -3.03276856161724

CubicTimeScaling(total_time, t)

Computes s(t) for a cubic time scaling.

Examples

julia> CubicTimeScaling(2, 0.6)
0.21600000000000003

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

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

EndEffectorForces(joint_positions, tip_wrench, link_frames, spatial_inertias, screw_axes)

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

Arguments

• joint_positions: the $n$-vector of joint variables.
• tip_wrench: the spatial force applied by the end-effector expressed in frame {n+1}.
• link_frames: the list of link frames i relative to i-1 at the home position.
• spatial_inertias: the spatial inertia matrices Gi of the links.
• screw_axes: 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 tip_wrench. This function calls InverseDynamics with gravity = 0, joint_velocities = 0, and joint_accelerations = 0.

Examples

julia> import LinearAlgebra as LA

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

julia> tip_wrench = [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> link_frames = [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> spatial_inertias = [G1, G2, G3]
3-element Vector{LinearAlgebra.Diagonal{Float64, Vector{Float64}}}:
 Diagonal([0.010267, 0.010267, 0.00666, 3.7, 3.7, 3.7])
 Diagonal([0.22689, 0.22689, 0.0151074, 8.393, 8.393, 8.393])
 Diagonal([0.0494433, 0.0494433, 0.004095, 2.275, 2.275, 2.275])

julia> screw_axes = [ 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(joint_positions, tip_wrench, link_frames, spatial_inertias, screw_axes)
3-element Vector{Float64}:
 1.4095460782639782
 1.8577149723180628
 1.392409

EulerStep(joint_positions, joint_velocities, joint_accelerations, timestep)

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

Arguments

• joint_positions: the $n$-vector of joint variables.
• joint_velocities: the $n$-vector of joint rates.
• joint_accelerations: the $n$-vector of joint accelerations.
• timestep: the timestep delta t.

Return

• joint_positionsNext: the vector of joint variables after timestep from first order Euler integration.
• joint_velocitiesNext: the vector of joint rates after timestep 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])

FKinBody(home_config, body_screw_axes, joint_positions)

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

Examples

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

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

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

julia> FKinBody(home_config, body_screw_axes, joint_positions)
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

FKinSpace(home_config, screw_axes, joint_positions)

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

Examples

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

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

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

julia> FKinSpace(home_config, screw_axes, joint_positions)
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

ForwardDynamics(joint_positions, joint_velocities, joint_torques, gravity, tip_wrench, link_frames, spatial_inertias, screw_axes)

Computes forward dynamics in the space frame for an open chain robot. Computes $\ddot{\theta}$ by solving $M(\theta)\ddot{\theta} = \tau - c(\theta,\dot{\theta}) - g(\theta) - J^T(\theta) \mathcal{F}_{\text{tip}}$.

Arguments

• joint_positions: the $n$-vector of joint variables.
• joint_velocities: the $n$-vector of joint rates.
• joint_torques: the $n$-vector of joint forces/torques.
• gravity: the gravity vector $g$ (e.g., [0, 0, -9.8]).
• tip_wrench: the wrench $\mathcal{F}_{\text{tip}}$ applied by the end-effector expressed in frame $\{n+1\}$.
• link_frames: a vector of $n+1$ SE(3) matrices, where link_frames[i] is $M_{i-1,i}$ and link_frames[n+1] is $M_{n,n+1}$.
• spatial_inertias: a vector of $n$ symmetric 6×6 spatial inertia matrices $G_i$ of the links.
• screw_axes: the screw axes $S_i$ of the joints in a space frame, as a 6×$n$ matrix with axes as columns.

Returns

The $n$-vector of joint accelerations $\ddot{\theta}$.

Examples

julia> ForwardDynamics([0.1, 0.1, 0.1], [0.1, 0.2, 0.3], [0.5, 0.6, 0.7], [0, 0, -9.8], [1, 1, 1, 1, 1, 1], link_frames, spatial_inertias, screw_axes)
3-element Vector{Float64}:
  -0.9739290670855625
  25.584667840340547
 -32.91499212478147

ForwardDynamicsTrajectory(joint_positions, joint_velocities, joint_torque_traj, gravity, tip_wrench_traj, link_frames, spatial_inertias, screw_axes, timestep, integration_resolution)

Simulates the motion of a serial chain given an open-loop history of joint forces/torques. Uses ForwardDynamics with EulerStep integration at each timestep.

Arguments

• joint_positions: the $n$-vector of initial joint variables.
• joint_velocities: the $n$-vector of initial joint velocities.
• joint_torque_traj: an $N×n$ matrix where row $i$ is the joint force/torque vector at timestep $i$.
• gravity: the gravity vector $g$ (e.g., [0, 0, -9.8]).
• tip_wrench_traj: an $N×6$ matrix where row $i$ is the spatial wrench applied by the end-effector at timestep $i$.
• link_frames: a vector of $n+1$ SE(3) matrices, where link_frames[i] is $M_{i-1,i}$ and link_frames[n+1] is $M_{n,n+1}$.
• spatial_inertias: a vector of $n$ symmetric 6×6 spatial inertia matrices $G_i$ of the links.
• screw_axes: the screw axes $S_i$ of the joints in a space frame, as a 6×$n$ matrix with axes as columns.
• timestep: the timestep $\Delta t$ between consecutive trajectory points.
• integration_resolution: the number of Euler integration steps during each timestep $\Delta t$. Must be a positive integer $\geq 1$. Larger values result in slower simulations but less accumulation of integration error.

Returns

• joint_position_traj: the $N×n$ matrix of joint variables resulting from the specified joint forces/torques.
• joint_velocity_traj: the $N×n$ matrix of joint velocities resulting from the specified joint forces/torques.

Examples

julia> joint_torque_traj = [3.63 -6.58 -5.57; 3.74 -5.55 -5.5; 4.31 -0.68 -5.19];

julia> thetamat, dthetamat = ForwardDynamicsTrajectory([0.1, 0.1, 0.1], [0.1, 0.2, 0.3], joint_torque_traj, [0, 0, -9.8], ones(3, 6), link_frames, spatial_inertias, screw_axes, 0.1, 8);

julia> thetamat
3×3 adjoint(::Matrix{Float64}) with eltype Float64:
 0.1       0.1        0.1
 0.106431  0.2626    -0.226649
 0.10198   0.715813  -1.22522

GravityForces(joint_positions, gravity, link_frames, spatial_inertias, screw_axes)

Computes the joint forces/torques an open chain robot requires to overcome gravity at its configuration. Calls InverseDynamics with joint_velocities = 0, joint_accelerations = 0, and tip_wrench = 0.

Arguments

• joint_positions: the $n$-vector of joint variables.
• gravity: the gravity vector $g$ (e.g., [0, 0, -9.8]).
• link_frames: a vector of $n+1$ SE(3) matrices, where link_frames[i] is $M_{i-1,i}$ and link_frames[n+1] is $M_{n,n+1}$.
• spatial_inertias: a vector of $n$ symmetric 6×6 spatial inertia matrices $G_i$ of the links.
• screw_axes: the screw axes $S_i$ of the joints in a space frame, as a 6×$n$ matrix with axes as columns.

Returns

The $n$-vector of joint gravity torques $g(\theta)$.

Examples

julia> GravityForces([0.1, 0.1, 0.1], [0, 0, -9.8], link_frames, spatial_inertias, screw_axes)
3-element Vector{Float64}:
  28.40331261821983
 -37.64094817177068
  -5.4415891999683605

IKinBody(body_screw_axes, home_config, target_config, initial_guess, angular_tolerance, linear_tolerance)

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

Examples

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

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

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

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

julia> angular_tolerance, linear_tolerance = 0.01, 0.001;

julia> IKinBody(body_screw_axes, home_config, target_config, initial_guess, angular_tolerance, linear_tolerance)
([1.5707381937148923, 2.999666997382943, 3.141539129217613], true)

IKinSpace(screw_axes, home_config, target_config, initial_guess, angular_tolerance, linear_tolerance)

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

Examples

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

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

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

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

julia> angular_tolerance, linear_tolerance = 0.01, 0.001;

julia> IKinSpace(screw_axes, home_config, target_config, initial_guess, angular_tolerance, linear_tolerance)
([1.57073782965672, 2.999663844672525, 3.141534199856583], true)

InverseDynamics(joint_positions, joint_velocities, joint_accelerations, gravity, tip_wrench, link_frames, spatial_inertias, screw_axes)

Computes inverse dynamics in the space frame for an open chain robot using forward-backward Newton-Euler iterations.

Arguments

• joint_positions: the $n$-vector of joint variables.
• joint_velocities: the $n$-vector of joint rates.
• joint_accelerations: the $n$-vector of joint accelerations.
• gravity: the gravity vector $g$ (e.g., [0, 0, -9.8]).
• tip_wrench: the wrench $\mathcal{F}_{\text{tip}}$ applied by the end-effector expressed in frame $\{n+1\}$.
• link_frames: a vector of $n+1$ SE(3) matrices, where link_frames[i] is $M_{i-1,i}$ and link_frames[n+1] is $M_{n,n+1}$ (the end-effector frame relative to the last link).
• spatial_inertias: a vector of $n$ symmetric 6×6 spatial inertia matrices $G_i$ of the links.
• screw_axes: the screw axes $S_i$ of the joints in a space frame, as a 6×$n$ matrix with axes as columns.

Returns

The $n$-vector of required joint forces/torques.

Examples

julia> InverseDynamics([0.1, 0.1, 0.1], [0.1, 0.2, 0.3], [2, 1.5, 1], [0, 0, -9.8], [1, 1, 1, 1, 1, 1], link_frames, spatial_inertias, screw_axes)
3-element Vector{Float64}:
  74.6961615528745
 -33.067660158514585
  -3.2305731379014246

InverseDynamicsTrajectory(joint_position_traj, joint_velocity_traj, joint_acceleration_traj, gravity, tip_wrench_traj, link_frames, spatial_inertias, screw_axes)

Calculates the joint forces/torques required to move the serial chain along the given trajectory using InverseDynamics at each timestep.

Arguments

• joint_position_traj: an $N×n$ matrix of joint variables, where row $i$ is the joint vector at timestep $i$.
• joint_velocity_traj: an $N×n$ matrix of joint velocities.
• joint_acceleration_traj: an $N×n$ matrix of joint accelerations.
• gravity: the gravity vector $g$ (e.g., [0, 0, -9.8]).
• tip_wrench_traj: an $N×6$ matrix where row $i$ is the spatial wrench applied by the end-effector at timestep $i$.
• link_frames: a vector of $n+1$ SE(3) matrices, where link_frames[i] is $M_{i-1,i}$ and link_frames[n+1] is $M_{n,n+1}$.
• spatial_inertias: a vector of $n$ symmetric 6×6 spatial inertia matrices $G_i$ of the links.
• screw_axes: the screw axes $S_i$ of the joints in a space frame, as a 6×$n$ matrix with axes as columns.

Returns

An $N×n$ matrix of joint forces/torques, where row $i$ is the joint force-torque at timestep $i$.

Examples

julia> traj_pos = [0.0 0.0 0.0; 0.5 0.5 0.5; 1.0 1.0 1.0];

julia> traj_vel = [0.0 0.0 0.0; 0.5 0.5 0.5; 1.0 1.0 1.0];

julia> traj_acc = [0.5 0.5 0.5; 0.5 0.5 0.5; 0.5 0.5 0.5];

julia> InverseDynamicsTrajectory(traj_pos, traj_vel, traj_acc, [0, 0, -9.8], ones(3, 6), link_frames, spatial_inertias, screw_axes)
3×3 adjoint(::Matrix{Float64}) with eltype Float64:
  23.7113  -35.2321  -3.87345
 105.336   -27.6441  -1.41169
 129.143   -17.3084   2.30991

JacobianBody(body_screw_axes, joint_positions)

Computes the body Jacobian for an open chain robot.

Examples

julia> body_screw_axes = [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> joint_positions = [0.2, 1.1, 0.1, 1.2];

julia> JacobianBody(body_screw_axes, joint_positions)
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

JacobianSpace(screw_axes, joint_positions)

Computes the space Jacobian for an open chain robot.

Examples

julia> screw_axes = [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> joint_positions = [0.2, 1.1, 0.1, 1.2];

julia> JacobianSpace(screw_axes, joint_positions)
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  

JointTrajectory(joint_position_start, joint_position_end, total_time, N, method)

Computes a straight-line trajectory in joint space with the specified time scaling.

Arguments

• joint_position_start: the $n$-vector of initial joint variables.
• joint_position_end: the $n$-vector of final joint variables.
• total_time: total time of the motion in seconds from rest to rest.
• N: the number of points $N \geq 2$ in the discrete representation of the trajectory.
• method: the time-scaling method — use 3 for cubic (CubicTimeScaling) or 5 for quintic (QuinticTimeScaling).

Returns

An $N×n$ matrix where each row is an $n$-vector of joint variables. The first row is joint_position_start and the $N$th row is joint_position_end. The elapsed time between each row is $T_f / (N - 1)$.

Examples

julia> JointTrajectory([0, 0], [1, 2], 2, 4, 3)
4×2 adjoint(::Matrix{Float64}) with eltype Float64:
 0.0       0.0
 0.259259  0.518519
 0.740741  1.48148
 1.0       2.0

MassMatrix(joint_positions, link_frames, spatial_inertias, screw_axes)

Computes the mass matrix of an open chain robot based on the given configuration. Calls InverseDynamics $n$ times, each time passing a $\ddot{\theta}$ vector with a single element equal to one and all other inputs set to zero. Each call generates a single column of $M(\theta)$.

Arguments

• joint_positions: the $n$-vector of joint variables.
• link_frames: a vector of $n+1$ SE(3) matrices, where link_frames[i] is $M_{i-1,i}$ and link_frames[n+1] is $M_{n,n+1}$.
• spatial_inertias: a vector of $n$ symmetric 6×6 spatial inertia matrices $G_i$ of the links.
• screw_axes: the screw axes $S_i$ of the joints in a space frame, as a 6×$n$ matrix with axes as columns.

Returns

The $n×n$ mass matrix $M(\theta)$.

Examples

julia> MassMatrix([0.1, 0.1, 0.1], link_frames, spatial_inertias, screw_axes)
3×3 Matrix{Float64}:
 22.5433      -0.307147  -0.00718426
 -0.307147     1.96851    0.432157
 -0.00718426   0.432157   0.191631

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 

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       

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   

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    

NearZero(z)

Determines whether a scalar is small enough to be treated as zero.

Examples

julia> NearZero(-1e-7)
true

Normalize(V)

Normalizes a vector.

Examples

julia> Normalize([1, 2, 3])
3-element Vector{Float64}:
 0.2672612419124244
 0.5345224838248488
 0.8017837257372732

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

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

QuinticTimeScaling(total_time, t)

Computes s(t) for a quintic time scaling.

Examples

julia> QuinticTimeScaling(2, 0.6)
0.16308

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

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

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

ScrewTrajectory(transform_start, transform_end, total_time, N, method)

Computes a trajectory as a list of $N$ SE(3) matrices corresponding to the screw motion about a space screw axis. Unlike CartesianTrajectory, the origin follows a screw path rather than a straight line.

Arguments

• transform_start: the initial end-effector configuration (4×4 SE(3) matrix).
• transform_end: the final end-effector configuration (4×4 SE(3) matrix).
• total_time: total time of the motion in seconds from rest to rest.
• N: the number of points $N \geq 2$ in the discrete representation of the trajectory.
• method: the time-scaling method — use 3 for cubic (CubicTimeScaling) or 5 for quintic (QuinticTimeScaling).

Returns

A list of $N$ SE(3) matrices separated in time by $T_f / (N - 1)$. The first in the list is transform_start and the $N$th is transform_end.

Examples

julia> Xstart = [1 0 0 1; 0 1 0 0; 0 0 1 1; 0 0 0 1];

julia> Xend = [0 0 1 0.1; 1 0 0 0; 0 1 0 4.1; 0 0 0 1];

julia> traj = ScrewTrajectory(Xstart, Xend, 5, 4, 3);

julia> length(traj)
4

julia> traj[1]
4×4 Matrix{Float64}:
 1.0  0.0  0.0  1.0
 0.0  1.0  0.0  0.0
 0.0  0.0  1.0  1.0
 0.0  0.0  0.0  1.0

SimulateControl(joint_positions, joint_velocities, gravity, tip_wrench_traj, link_frames, spatial_inertias, screw_axes, desired_joint_position_traj, desired_joint_velocity_traj, desired_joint_acceleration_traj, estimated_gravity, estimated_link_frames, estimated_spatial_inertias, Kp, Ki, Kd, timestep, integration_resolution)

Simulates the ComputedTorque controller over a given desired trajectory using ForwardDynamics and numerical integration. The controller may use different (possibly inaccurate) dynamics parameters (estimated_gravity, estimated_link_frames, estimated_spatial_inertias) while the actual forward dynamics simulation uses the true parameters (gravity, link_frames, spatial_inertias).

Arguments

• joint_positions: the $n$-vector of initial joint variables.
• joint_velocities: the $n$-vector of initial joint velocities.
• gravity: the actual gravity vector used in the forward dynamics simulation.
• tip_wrench_traj: an $N×6$ matrix where row $i$ is the spatial wrench applied by the end-effector at timestep $i$.
• link_frames: the actual link frames (vector of $n+1$ SE(3) matrices) used in the forward dynamics simulation.
• spatial_inertias: the actual spatial inertia matrices (vector of $n$ 6×6 matrices) used in the forward dynamics simulation.
• screw_axes: the screw axes $S_i$ of the joints in a space frame, as a 6×$n$ matrix with axes as columns.
• desired_joint_position_traj: an $N×n$ matrix of desired joint positions at each timestep.
• desired_joint_velocity_traj: an $N×n$ matrix of desired joint velocities at each timestep.
• desired_joint_acceleration_traj: an $N×n$ matrix of desired joint accelerations at each timestep.
• estimated_gravity: the gravity vector used by the controller (may differ from gravity).
• estimated_link_frames: the link frames used by the controller (may differ from link_frames).
• estimated_spatial_inertias: the spatial inertia matrices used by the controller (may differ from spatial_inertias).
• Kp: the proportional gain (scalar).
• Ki: the integral gain (scalar).
• Kd: the derivative gain (scalar).
• timestep: the timestep $\Delta t$ between trajectory reference points.
• integration_resolution: the number of Euler integration steps per timestep.

Returns

• joint_torque_traj: an $N×n$ matrix of applied joint torques at each timestep.
• joint_position_traj: an $N×n$ matrix of actual joint positions at each timestep.

Examples

julia> desired_pos = [0.1 0.1 0.1; 0.2 0.2 0.2; 0.3 0.3 0.3];

julia> desired_vel = [0.1 0.1 0.1; 0.1 0.1 0.1; 0.1 0.1 0.1];

julia> taumat, thetamat = SimulateControl([0.1, 0.1, 0.1], [0.1, 0.2, 0.3], [0, 0, -9.8], ones(3, 6), link_frames, spatial_inertias, screw_axes, desired_pos, desired_vel, zeros(3, 3), [0, 0, -9.8], link_frames, spatial_inertias, 20, 10, 18, 0.1, 4);

julia> taumat
3×3 adjoint(::Matrix{Float64}) with eltype Float64:
 29.2466  -42.7951  -6.91623
 93.5113  -24.4938  -0.00376585
 45.3612  -39.6324  -5.62033

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

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

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

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])

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

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

VelQuadraticForces(joint_positions, joint_velocities, link_frames, spatial_inertias, screw_axes)

Computes the Coriolis and centripetal terms $c(\theta, \dot{\theta})$ in the inverse dynamics of an open chain robot. Calls InverseDynamics with gravity = 0, tip_wrench = 0, and joint_accelerations = 0.

Arguments

• joint_positions: the $n$-vector of joint variables.
• joint_velocities: the $n$-vector of joint rates.
• link_frames: a vector of $n+1$ SE(3) matrices, where link_frames[i] is $M_{i-1,i}$ and link_frames[n+1] is $M_{n,n+1}$.
• spatial_inertias: a vector of $n$ symmetric 6×6 spatial inertia matrices $G_i$ of the links.
• screw_axes: the screw axes $S_i$ of the joints in a space frame, as a 6×$n$ matrix with axes as columns.

Returns

The $n$-vector of Coriolis and centripetal terms.

Examples

julia> VelQuadraticForces([0.1, 0.1, 0.1], [0.1, 0.2, 0.3], link_frames, spatial_inertias, screw_axes)
3-element Vector{Float64}:
  0.26453118054501235
 -0.0550515682891655
 -0.006891320068248912

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

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

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