A powerful library for robotics analysis 🦾 🤖
- Index 🗂️
- Introduction 📚
- Features ✨
- Future Work ☁️
- Before Starting
⚠️ - Library Content 📖
- Basic Frame Movements
- Dual Quaternions Functionalities
- Robot Creation and Setup 🦿
- Kinematics
- Differential Kinematics
- Dynamics
You can use this simple library to analyze and develop kinematics and control algorithms for your robots as shown in our online trainigs:
- Numerical Methods with Engineering Applications
- Control of Dynamical Systems
- Robotics: from Kinematics to Control
We hope this library will help you to start your journey in these amazing discipline! ❤️
You can set your robot attributes and analyze its behavior. To achieve this, all the algorithms were developed using Homogeneous Transformation Matrices and Dual Quaternions algebra, however, the logic used to develop them will allow you to adapt it to almost any embedded system!
For serial robots (we will include new ones in the future 😉), some interesting functionalities are listed below:
- Forward Kinematics
- Using Homogeneous Transformation Matrices (numerical and symbolic)
- Using Dual Quaternions (numerical and symbolic)
- Numerical Inverse Kinematics
- Differential Kinematics
- Using Homogeneous Transformation Matrices (numerical and symbolic)
- Using Dual Quaternions (numerical and symbolic)
- Robot Dynamics
- Differential Equation using Homogeneous Transformation Matrices (numerical and symbolic)
- Differential Equation using Dual Quaternions (numerical and symbolic)
Feel free to modify, adjust and extend our work to your necessities 😃; this library allows you to get a first approach to robot analysis, synthesis and control, however, we will be adding new interesting features, also, you can request new features or create new ones!
We are working, or will start working soon, on the following tasks for future releases:
- Velocity Recursive Algorithms
- Using Homogeneous Transformation Matrices
- Using Dual Quaternions
- Acceleration Recursive Algorithms
- Using Homogeneous Transformation Matrices
- Using Dual Quaternions
- Euler - Lagrange formulation
- Using Homogeneous Transformation Matrices
- Dynamic Model in Joints Space
- Dynamic Model in Cartesian Space
- Using Dual Quaternions
- Dynamic Model in Joints Space
- Dynamic Model in Cartesian Space
- Using Homogeneous Transformation Matrices
- Newton - Euler Recursive Algorithm:
- Using Homogeneous Transformation Matrices
- Using Dual Quaternions
This library can be used with Python 🐍 3.10.4 - 64 bits or above. Please install the following dependencies or make sure that they're already in your computer:
pip3 install numpy
pip3 install sympy
pip3 install matplotlibPlease do not forget to set your Python Path 😉. Also, you can use pip depending on your Python version and/or configuration:
pip install numpy
pip install sympy
pip install matplotlibIf these modules were installed correctly, you will be able to use our library 😃
You can get your robot's equations in a symbolic form, however, please consider that this computation is slower than numerical one, also resulting equations won't be simplified most of the times as we do in paper. If you need to simplify the obtained results, you can use trigsimp() function as follows:
"""
Example of symbolic simplification
"""
# Homogeneous Transformation Matrix library
from lib.movements.HTM import *
# SymPy library
from sympy import *
# Symbolical Homogeneous Transformation Matrix (multiplication is done with * because is a symbolic matrix)
H = rz(z = "q1", symbolic = True) * tx(x = "L1", symbolic = True) * rz(z = "q2", symbolic = True) * tx(x = "L2", symbolic = True)
# Simplified Homogeneous Transformation Matrix
Hsimplified = trigsimp(H)So the outputs will be
>>> H
Matrix([[-sin(q1)*sin(q2) + cos(q1)*cos(q2), -sin(q1)*cos(q2) - sin(q2)*cos(q1), 0, L1*cos(q1) + L2*(-sin(q1)*sin(q2) + cos(q1)*cos(q2))],
[ sin(q1)*cos(q2) + sin(q2)*cos(q1), -sin(q1)*sin(q2) + cos(q1)*cos(q2), 0, L1*sin(q1) + L2*(sin(q1)*cos(q2) + sin(q2)*cos(q1))],
[ 0, 0, 1, 0],
[ 0, 0, 0, 1]])
>>> Hsimplified
Matrix([[cos(q1 + q2), -sin(q1 + q2), 0, L1*cos(q1) + L2*cos(q1 + q2)],
[sin(q1 + q2), cos(q1 + q2), 0, L1*sin(q1) + L2*sin(q1 + q2)],
[ 0, 0, 1, 0],
[ 0, 0, 0, 1]])Please consider that the bigger the equation, the slower the simplification. Also trigsimp() will find the best possible value, then sometimes your simplification won't be the same as the computer's, but it doesn't mean equations are wrong, but they are equivalent 😉
This library includes the following files:
zRobotics
├── img # Images for README.md file and related ones
├── lib # Python source files
| ├── Robot.py # Robot constructor
| ├── movements
| | ├── DQ.py # Rotations, translations and other functionalities for Dual Quaternios
| | └── HTM.py # Rotations, translations and other functionalities for Homogeneous Transformation Matrices
| ├── kinematics
| | ├── DQ.py # Forward and Inverse Kinematics functions using Dual Quaternios
| | ├── HTM.py # Forward and Inverse Kinematics functions using Homogeneous Transformation Matrices
| | ├── DifferentialDQ.py # Velocities and accelerations using Dual Quaternions
| | └── DifferentialHTM.py # Velocities and accelerations using Homogeneous Transformation Matrices
| ├── dynamics
| | ├── DynamicsHTM.py # Dynamic equations using Homogeneous Transformation Matrices
| | ├── DynamicsDQ.py # Dynamic equations using Dual Quaternions
| | └── Solver.py # Numerical solvers for multiple purposes (UNDER DEVELOPMENT)
| ├── plot
| └── └── Plot.py # For function plotting (UNDER DEVELOPMENT)
├── CODE_OF_CONDUCT.md
├── README.md # Users' manual
├── LICENSE
└── main.py # File to demonstrate functionalities
New functionalities can be added in future releases, so feel free to send an email to contact@zdynamics.org requesting anything new 😃
Just open your terminal (depending on your operating system) inside zRobotics directory, then run python or python3 and type from lib.NameOfLibrary.NameOfFile import *, for example
from lib.movements.HTM import *So this will import the movements library that uses Homogeneous Transformations Matrices. If you have a Python project in another directory, just copy the lib folder and paste it into your project's directory:
your_project_root_directory
├── folder_1
| ├──image.jpg
| └── file0.c
├── folder_2
| ├── test.ino
| └── file1.py
├── lib # zRobotics Python source files
└── file2.py
Then, you will be able to use all our files from your root directory 😉
lib.Robot: this creates your robot as an object 🤖 to facilitate algorithms implementation and usage, so what you have to do is to create its Denavit - Hartenberg Parameters Matrix.⚠️ It only allows to create serial manipulators⚠️ , but some other robots will be added soonlib.movements: it can be used to compute the translational and rotational movements representation using Homogeneous Transformation Matrices or Dual Quaternionslib.kinematics: based on Dual Quaternions algebra and Homogeneous Transformation Matrices, it solves the most common kinematics problems: from forward kinematics to differential one (under construction 🚧)-
lib.dynamics: it has numerical integration and dynamical systems algorithms (under construction 🚧) lib.plot: this allows to plot graphs of your robot's behavior (under construction 🚧)
Please take a look at main.py to know more about the commands, their implementation and the order they could be executed. Feel free to contact us if you have any comment, suggestion or question 😄
You can compute translational or rotational movements with Homogeneous Transformation Matrices and Dual Quaternions
You can compute translational movements on each axis of euclidian space: (x, y, z)
"""
Translational movement on "Y" axis
"""
# Homogeneous Transformation Matrix library
from lib.movements.HTM import *
# Dual Quaternions library
from lib.movements.DQ import *
# Using Homogeneous Transformation Matrix
H = ty(y = 0.5)
# Using Dual Quaternions
Q = dqTy(y = 0.5)So the outputs will be
# NumPy Array
>>> H
array([[1. , 0. , 0. , 0. ],
[0. , 1. , 0. , 0.5],
[0. , 0. , 1. , 0. ],
[0. , 0. , 0. , 1. ]])
# NumPy Array
>>> Q
array([[1. ],
[0. ],
[0. ],
[0. ],
[0. ],
[0. ],
[0.25],
[0. ]])In case you need a symbolic expression, it's possible to call the same functions with a symbolic parameter:
"""
Translational movement on "Z" axis
"""
# Homogeneous Transformation Matrix library
from lib.movements.HTM import *
# Dual Quaternions library
from lib.movements.DQ import *
# Using Homogeneous Transformation Matrix
H = tz(z = 0.5, symbolic = True)
# Using Dual Quaternions
Q = dqTz(z = 0.5, symbolic = True)Then the outputs will be shown as follows:
# SymPy Matrix
>>> H
Matrix([[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 0.5],
[0, 0, 0, 1]])
# SymPy Matrix
>>> Q
Matrix([[ 1],
[ 0],
[ 0],
[ 0],
[ 0],
[ 0],
[ 0],
[0.25]])The functions for translational movements (with their default values) are shown below:
"""
Translational movements
"""
# Homogeneous Transformation Matrix library
from lib.movements.HTM import *
# Using Homogeneous Transformation Matrix
Hx = tx(x = 0, symbolic = False)
Hy = ty(y = 0, symbolic = False)
Hz = tz(z = 0, symbolic = False)
# Dual Quaternions library
from lib.movements.DQ import *
# Using Dual Quaternions
Qx = dqTx(x = 0.5, symbolic = True)
Qy = dqTy(y = 0.5, symbolic = True)
Qz = dqTz(z = 0.5, symbolic = True)You can compute rotational movements on each axis of euclidian space:
"""
Rotational movement on "Y" axis
"""
# Homogeneous Transformation Matrix library
from lib.movements.HTM import *
# Dual Quaternions library
from lib.movements.DQ import *
# Using Homogeneous Transformation Matrix
H = ry(y = 0.5)
# Using Dual Quaternions
Q = dqRy(y = 0.5)So the outputs will be
# NumPy Array
>>> H
array([[ 0.87758256, 0. , 0.47942554, 0. ],
[ 0. , 1. , 0. , 0. ],
[-0.47942554, 0. , 0.87758256, 0. ],
[ 0. , 0. , 0. , 1. ]])
# NumPy Array
>>> Q
array([[0.96891242],
[0. ],
[0.24740396],
[0. ],
[0. ],
[0. ],
[0. ],
[0. ]])In case you need a symbolic expression, it's possible to call the same functions with a symbolic parameter:
"""
Rotational movement on "Z" axis
"""
# Homogeneous Transformation Matrix library
from lib.movements.HTM import *
# Dual Quaternions library
from lib.movements.DQ import *
# Using Homogeneous Transformation Matrix
H = rz(z = 0.5, symbolic = True)
# Using Dual Quaternions
Q = dqRz(z = 0.5, symbolic = True)Then the outputs will be shown as follows:
# SymPy Matrix
>>> H
Matrix([
[0.877582561890373, -0.479425538604203, 0, 0],
[0.479425538604203, 0.877582561890373, 0, 0],
[ 0, 0, 1, 0],
[ 0, 0, 0, 1]])
# SymPy Matrix
>>> Q
Matrix([[0.968912421710645],
[ 0],
[ 0],
[0.247403959254523],
[ 0],
[ 0],
[ 0],
[ 0]])The functions for rotational movements (with their default values) are shown below:
"""
Rotational movements
"""
# Homogeneous Transformation Matrix library
from lib.movements.HTM import *
# Using Homogeneous Transformation Matrix
Hx = rx(x = 0, symbolic = False)
Hy = ry(y = 0, symbolic = False)
Hz = rz(z = 0, symbolic = False)
# Dual Quaternions library
from lib.movements.DQ import *
# Using Dual Quaternions
Qx = dqRx(x = 0.5, symbolic = False)
Qy = dqRy(y = 0.5, symbolic = False)
Qz = dqRz(z = 0.5, symbolic = False)As shown in our online training, there are multiple operations and operators for dual quaternions. Our library needs some of their functionalities to work correctly, so they are listed below
Let's consider two dual quaternions that have to be multiplicated as
, so this operation can be simplified with left and right operators
:
Then these operators can be calculated as follows:
"""
Dual Quaternions Multiplication
"""
# Dual Quaternions library
from lib.movements.DQ import *
# NumPy Library for Matrix multiplication
import numpy as np
# Dual Quaternion
Qa = dqTy(y = 0.5)
# Dual Quaternion
Qb = dqRz(z = 0.5)
# Multiplication with Left Operator
QcLeft = leftOperator(Q = Qa, symbolic = False).dot(Qb)
# Multiplication with Right Operator
QcRight = rightOperator(Q = Qb, symbolic = False).dot(Qa)So the outputs will be
# NumPy Array
>>> QcLeft
array([[0.96891242],
[0. ],
[0. ],
[0.24740396],
[0. ],
[0.06185099],
[0.24222811],
[0. ]])
# NumPy Array
>>> QcRight
array([[0.96891242],
[0. ],
[0. ],
[0.24740396],
[0. ],
[0.06185099],
[0.24222811],
[0. ]])This can be operated using symbolic expressions by setting symbolic parameter to True. however, it might be slow because it has to create an 8x8 matrix, so we recommend to use the following command that is faster (for both numeric and symbolic calculation):
"""
Faster Dual Quaternions Multiplication
"""
# Dual Quaternions library
from lib.movements.DQ import *
# NumPy Library for Matrix multiplication
import numpy as np
# Dual Quaternion
Qa = dqTy(y = 0.5)
# Dual Quaternion
Qb = dqRz(z = 0.5)
# Multiplication between two dual quaternions
Qc = dqMultiplication(Qa, Qb, symbolic = False)So the output will be
# NumPy Array
>>> Qc
array([[0.96891242],
[0. ],
[0. ],
[0.24740396],
[0. ],
[0.06185099],
[0.24222811],
[0. ]])You can also calculate its symbolic expressions by setting symbolic parameter to True.
The inverse form of an Homogeneous Transformation Matrix can be calculated easily with numpy library, however, the inverse form of a dual quaternion is as simple as conjugating its values:
"""
Dual Quaternion Conjugate
"""
# Dual Quaternions library
from lib.movements.DQ import *
# Dual Quaternion
Qa = dqTy(y = 0.5)
# Conjugate form of a Dual Quaternion
Qb = conjugateDQ(Q = Qa, symbolic = False)So the outputs will be
# NumPy Array
>>> Qa
array([[1. ],
[0. ],
[0. ],
[0. ],
[0. ],
[0. ],
[0.25],
[0. ]])
>>> Qb
array([[ 1. ],
[-0. ],
[-0. ],
[-0. ],
[ 0. ],
[-0. ],
[-0.25],
[-0. ]])You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
Sometimes it will be necessary to transform a pose representation as dual quaternion into an Euclidian space one
:
"""
Dual Quaternions to R3
"""
# Dual Quaternions library
from lib.movements.DQ import *
# Dual Quaternion
Q = dqTy(y = 0.5)
# Euclidian space representation
r = dqToR3(Q, symbolic = False)So the outputs will be
# NumPy Array
>>> Q
array([[1. ],
[0. ],
[0. ],
[0. ],
[0. ],
[0. ],
[0.25],
[0. ]])
>>> r
array([0. , 0. , 0.5, 0. ])You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
A SERIAL ROBOT can be created as an object (we will add other type of robots soon 😉). Before creating your system, it is necessary to set some attributes.
Serial manipulators are built by a Denavit - Hartenberg Parameters Matrix like this
| 0 | 0 | 0 | 0 |
| 0 | |||
| 0 | 0 | ||
| 0 | 0 | ||
| 0 | 0 |
so is mandatory to modify Robot.py with your robot's information, as you would do it in a sheet of paper (do not forget to include inertial frame):
def denavitHartenberg(self, symbolic = False):
"""Denavit - Hartenberg parameters for n - th rigid body
theta: rotation on «z» axis
d: translation on «z» axis
a: translation on «x» axis
alpha: rotation on «x» axis
"""
if symbolic:
# Set symbolic Denavit Hartenberg Parameters Matrix
self.symbolicDHParameters = Matrix([[0, 0, 0, 0],
[self.qSymbolic[0, 0], self.symbolicLinks[0], 0.0000000000000000000, np.pi / 2],
[self.qSymbolic[1, 0], 0.0000000000000000000, self.symbolicLinks[1], 0.0000000],
[self.qSymbolic[2, 0], 0.0000000000000000000, 0.0000000000000000000, np.pi / 2],
[self.qSymbolic[3, 0], self.symbolicLinks[2], 0.0000000000000000000, 0.0000000]])
else:
# Set numeric Denavit Hartenberg Parameters Matrix
self.dhParameters = np.array([[0, 0, 0, 0],
[self.jointsPositions[0, 0], self.linksLengths[0], 0.000000000000000000, np.pi / 2],
[self.jointsPositions[1, 0], 0.000000000000000000, self.linksLengths[1], 0.0000000],
[self.jointsPositions[2, 0], 0.000000000000000000, 0.000000000000000000, np.pi / 2],
[self.jointsPositions[3, 0], self.linksLengths[2], 0.000000000000000000, 0.0000000]])We included the following types of robots in Robot.py file:
- Two-link planar robot arm (click on the image for further information)
- Three-link spatial robot arm (click on the image for further information)
- Four degrees-of-freedom robot developed in our online training 😃
so take a look at it and uncomment the parameters of the robot that you want to work with 😉. It is not necessary to call this function before performing any kinematics task, this is because all the algorithms will access to these methods automatically. For future releases, we will work on a simpler way to create this matrices 😉
Some calculations in robotics needs to be performed with respect to the Center of Mass of the rigid bodies, so you have to modify Robot's file with your robot's information, as you would do it in a sheet of paper (do not forget to include inertial frame). For example:
| 0 | 0 | 0 | 0 |
| 0 | |||
| 0 | 0 | ||
| 0 | 0 | ||
| 0 | 0 |
Therefore,
def denavitHartenbergCOM(self, symbolic = False):
"""Denavit - Hartenberg parameters for n - th center of mass
theta: rotation on «z» axis
d: translation on «z» axis
a: translation on «x» axis
alpha: rotation on «x» axis
"""
if symbolic:
# Set symbolic Denavit Hartenberg Parameters Matrix
self.symbolicDHParametersCOM = Matrix([[0, 0, 0, 0],
[self.qSymbolic[0, 0], self.symbolicCOMs[0], 0.000000000000000000, np.pi / 2],
[self.qSymbolic[1, 0], 0.000000000000000000, self.symbolicCOMs[1], 0.0000000],
[self.qSymbolic[2, 0], 0.000000000000000000, 0.000000000000000000, np.pi / 2],
[self.qSymbolic[3, 0], self.symbolicCOMs[2], 0.000000000000000000, 0.0000000]])
else:
# Set numeric Denavit Hartenberg Parameters Matrix
self.dhParametersCOM = np.array([[0, 0, 0, 0],
[self.jointsPositions[0, 0], self.COMs[0], 0.0000000000, np.pi / 2],
[self.jointsPositions[1, 0], 0.0000000000, self.COMs[1], 0.0000000],
[self.jointsPositions[2, 0], 0.0000000000, 0.0000000000, np.pi / 2],
[self.jointsPositions[3, 0], self.COMs[2], 0.0000000000, 0.0000000]])As it happens with conventional Denavit - Hartenbger Parameters, it is not necessary to compute this matrix for centers of mass before executing any kinematics task. For future releases, we will work on a simpler way to create this matrices 😉
These are generated randomly as an example, but you can use your own values for each attribute 😃
# NumPy library is necessary for joints positions
import numpy as np
# Number of rigid bodies
rb = 3
# Number of Generalized Coordinates
n = 4
# Generalized coordinates
q = np.random.randn(n, 1)
# Joints velocities
qd = np.random.randn(n, 1)
# Joints accelerations
qdd = np.random.randn(n, 1)
# Screw vectors (or axes of actuation) stored in a matrix. This is MANDATORY for calculations using Dual Quaternions
xi = np.array([[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[1, 1, 1, 1],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 0]])
# Derivative of previous screw vectors (or axes of actuation) stored in a matrix. This is MANDATORY for calculations using Dual Quaternions
xid = np.zeros((8, n))
# Links
L = [np.random.rand() for body in range(rb)]
# Center of Mass of each link
Lcom = [value / 2 for value in L]
# Mass of each link
m = [np.random.rand() for i in range(rb)]
# Inertia of each link (with respect to the Center of Mass; it has to be a SYMMETRIC matrix)
Inertia = np.random.rand(3, 3)
Inertia = [0.5 * (Inertia + Inertia.T) for i in range(rb)]Where each attribute is described below:
- Joints positions, velocities and accelerations (generalized coordinates):
(set in
)
- Links Lengths and distance to Centers of Mass:
(set in meters)
- Screw vectors for Dual Quaternions operations:
- Mass of each link:
(set in kilograms)
- Inertia tensor of each link with respect to the origin of the rigid body:
(set in
)
It is really simple to create a robot object:
"""
Create a robot as an object
"""
# Robot's library
from lib.Robot import *
# Returns robot as an object
uRobot = Serial(jointsPositions = q, jointsVelocities = qd, jointsAccelerations = qdd, linksLengths = L, COMs = Lcom, mass = m, inertia = Inertia, name = 'uRobot', xi = xi, xid = xid)After this, it is possible to access to its attributes as follows:
# NumPy Array
>>> uRobot.jointsPositions
array([[0.46727453],
[0.54455365],
[0.4350795 ],
[0.28259213]])
# SymPy Matrix
>>> uRobot.qSymbolic
Matrix([[q1],
[q2],
[q3],
[q4]])
# List
>>> uRobot.linksLengths
[0.3, 0.4, 0.2]
# SymPy Matrix
>>> uRobot.symbolicLinks
Matrix([[L1],
[L2],
[L3]])
# List
>>> uRobot.COMs
[0.3, 0.4, 0.2]
# SymPy Matrix
>>> uRobot.symbolicCOMs
Matrix([[Lcom1],
[Lcom2],
[Lcom3]])The rest of attributes were ommitted, but you can check them in Robot.py. You can also add new attributes if they are necessary for your project 😃
To know the position of each reference frame in the robot by means of the Denavit - Hartenberg Parameters matrix, we can use the following functions
"""
Calculate forward kinematics
"""
# Kinematics libraries
from lib.kinematics.HTM import *
from lib.kinematics.DQ import *
# Robot's forward kinematics using Homogeneous Transformation Matrices (numerical and symbolic)
fkHTM = forwardHTM(uRobot)
symbolicFKHTM = forwardHTM(uRobot, symbolic = True)
# Robot's forward kinematics using Dual Quaternions (numerical and symbolic)
fkDQ = forwardDQ(uRobot)
symbolicFKDQ = forwardDQ(uRobot, symbolic = True)Where and
are lists that store the pose representation for each reference frame with Homogeneous Transformation Matrices or Dual Quaternions. You can access to each specific pose representation by indexing it:
# NumPy Array
>>> fkHTM[-1]
array([[ 0.60345261, 0.29383926, 0.74128499, 0.45372242],
[-0.0078605 , -0.92739162, 0.37400935, 0.22892198],
[ 0.79736012, -0.23152379, -0.55732717, 0.39574911],
[ 0. , 0. , 0. , 1. ]])
# SymPy Matrix
>>> symbolicFKHTM[-1]
Matrix([[sin(q1)*sin(q4) + cos(q1)*cos(q4)*cos(q2 + q3), sin(q1)*cos(q4) - sin(q4)*cos(q1)*cos(q2 + q3), sin(q2 + q3)*cos(q1), (L2*cos(q2) + L3*sin(q2 + q3))*cos(q1)],
[sin(q1)*cos(q4)*cos(q2 + q3) - sin(q4)*cos(q1), -sin(q1)*sin(q4)*cos(q2 + q3) - cos(q1)*cos(q4), sin(q1)*sin(q2 + q3), (L2*cos(q2) + L3*sin(q2 + q3))*sin(q1)],
[ sin(q2 + q3)*cos(q4), -sin(q4)*sin(q2 + q3), -cos(q2 + q3), L1 + L2*sin(q2) - L3*cos(q2 + q3)],
[ 0, 0, 0, 1]])
# NumPy Array
>>> fkDQ[-1]
array([[-0.17228887],
[ 0.87865969],
[ 0.0813679 ],
[ 0.43778186],
[-0.29527314],
[-0.00507735],
[ 0.05482832],
[-0.11620462]])
# SymPy Matrix
>>> symbolicFKDQ[-1]
Matrix([[ -sin(q1/2 + q4/2)*sin(q2/2 + q3/2)],
[ cos(q1/2 - q4/2)*cos(q2/2 + q3/2)],
[ sin(q1/2 - q4/2)*cos(q2/2 + q3/2)],
[ sin(q2/2 + q3/2)*cos(q1/2 + q4/2)],
[-(L1*sin(q2/2 + q3/2) + L2*cos(q2/2 - q3/2) + L3*sin(q2/2 + q3/2))*cos(q1/2 + q4/2)/2],
[(-L1*cos(q2/2 + q3/2) - L2*sin(q2/2 - q3/2) + L3*cos(q2/2 + q3/2))*sin(q1/2 - q4/2)/2],
[ (L1*cos(q2/2 + q3/2) + L2*sin(q2/2 - q3/2) - L3*cos(q2/2 + q3/2))*cos(q1/2 - q4/2)/2],
[-(L1*sin(q2/2 + q3/2) + L2*cos(q2/2 - q3/2) + L3*sin(q2/2 + q3/2))*sin(q1/2 + q4/2)/2]])IMPORTANT NOTE: Please notice that symbolic computation is slower than numerical one, so use those commands only if you need to know the equations of your system. Take a look at Symbolic Computation for more information 😉
"""
Calculate forward kinematics to each center of mass
"""
# Kinematics libraries
from lib.kinematics.HTM import *
from lib.kinematics.DQ import *
# Robot's forward kinematics to each center of mass using Homogeneous Transformation Matrices (numerical and symbolic)
fkCOMHTM = forwardCOMHTM(uRobot)
symbolicFKCOMHTM = forwardCOMHTM(uRobot, symbolic = True)
# Robot's forward kinematics to each center of mass using Dual Quaternions (numerical and symbolic)
fkCOMDQ = forwardCOMDQ(uRobot)
symbolicDQCOMHTM = forwardCOMDQ(uRobot, symbolic = True)Where and
are lists that store the pose representation for each center of mass with Homogeneous Transformation Matrices or Dual Quaternions. You can get all the elements of the list, but also you can access to each specific pose representation by indexing it:
# NumPy Array
>>> fkCOMHTM[-1]
array([[ 6.03452611e-01, 2.93839264e-01, 7.41284988e-01, 4.53722422e-01],
[-7.86050467e-03, -9.27391622e-01, 3.74009346e-01, 2.28921979e-01],
[ 7.97360119e-01, -2.31523791e-01, -5.57327171e-01, 3.95749108e-01],
[-1.99892919e-16, -6.93639930e-18, 1.57048749e-17, 1.00000000e+00]])
# SymPy Matrix
>>> symbolicFKCOMHTM[-1]
Matrix([[sin(q1)*sin(q4) + cos(q1)*cos(q4)*cos(q2 + q3), sin(q1)*cos(q4) - sin(q4)*cos(q1)*cos(q2 + q3), sin(q2 + q3)*cos(q1), (L2*cos(q2) + Lcom3*sin(q2 + q3))*cos(q1)],
[sin(q1)*cos(q4)*cos(q2 + q3) - sin(q4)*cos(q1), -sin(q1)*sin(q4)*cos(q2 + q3) - cos(q1)*cos(q4), sin(q1)*sin(q2 + q3), (L2*cos(q2) + Lcom3*sin(q2 + q3))*sin(q1)],
[ sin(q2 + q3)*cos(q4), -sin(q4)*sin(q2 + q3), -cos(q2 + q3), L1 + L2*sin(q2) - Lcom3*cos(q2 + q3)],
[ 0, 0, 0, 1]])
# NumPy Array
>>> fkCOMDQ[-1]
array([[-0.17228887],
[ 0.87865969],
[ 0.0813679 ],
[ 0.43778186],
[-0.29527314],
[-0.00507735],
[ 0.05482832],
[-0.11620462]])
# SymPy Matrix
>>> symbolicDQCOMHTM[-1]
Matrix([[ -sin(q1/2 + q4/2)*sin(q2/2 + q3/2)],
[ cos(q1/2 - q4/2)*cos(q2/2 + q3/2)],
[ sin(q1/2 - q4/2)*cos(q2/2 + q3/2)],
[ sin(q2/2 + q3/2)*cos(q1/2 + q4/2)],
[-(L1*sin(q2/2 + q3/2) + L2*cos(q2/2 - q3/2) + Lcom3*sin(q2/2 + q3/2))*cos(q1/2 + q4/2)/2],
[(-L1*cos(q2/2 + q3/2) - L2*sin(q2/2 - q3/2) + Lcom3*cos(q2/2 + q3/2))*sin(q1/2 - q4/2)/2],
[ (L1*cos(q2/2 + q3/2) + L2*sin(q2/2 - q3/2) - Lcom3*cos(q2/2 + q3/2))*cos(q1/2 - q4/2)/2],
[-(L1*sin(q2/2 + q3/2) + L2*cos(q2/2 - q3/2) + Lcom3*sin(q2/2 + q3/2))*sin(q1/2 + q4/2)/2]])In this case, and
are defined as
and
respectively.
IMPORTANT NOTE: Please notice that symbolic computation is slower than numerical one, so use those commands only if you need to know the equations of your system. Take a look at Symbolic Computation for more information 😉
A compact representation of an Homogeneous Transformation Matrix can be obtained by and Axis - Angle Vector . This is OPTIONAL, because each function can call it automatically if needed:
# Kinematics libraries
from lib.kinematics.HTM import *
# Axis - Angle vector based on Homogeneous Transformation Matrix obtained by Forward Kinematics calculation
X = axisAngle(fkHTM[-1])
symbolicX = axisAngle(symbolicFKHTM[-1], symbolic = True)So the output will be:
# NumPy Array
>>> X
array([[ 0.48684051],
[ 0.07557617],
[ 0.19911015],
[-2.55307359],
[ 1.13388496],
[-0.6245545 ]])
# SymPy Matrix
>>> symbolicX
Matrix([[ (L2*cos(q2) + L3*sin(q2 + q3))*cos(q1)],
[ (L2*cos(q2) + L3*sin(q2 + q3))*sin(q1)],
[ L1 + L2*sin(q2) - L3*cos(q2 + q3)],
[-(sin(q1) + sin(q4))*sin(q2 + q3)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)],
[ (cos(q1) - cos(q4))*sin(q2 + q3)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)],
[ (cos(q2 + q3) - 1)*sin(q1 + q4)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)]])IMPORTANT NOTE: Please notice that symbolic computation is slower than numerical one, so use those commands only if you need to know the equations of your system. Take a look at Symbolic Computation for more information 😉
This is OPTIONAL, because each function that needs an Inertial Jacobian Matrix can call and process it automatically 😉 but you can calculate its geometrical or analytical form as follows:
# Kinematics libraries
from lib.kinematics.HTM import *
# Geometric Jacobian Matrix (OPTIONAL)
Jg = geometricJacobian(uRobot)
symbolicJg = geometricJacobian(uRobot, symbolic = True)
# Analytic Jacobian Matrix (OPTIONAL)
Ja = analyticJacobian(uRobot)
symbolicJa = analyticJacobian(uRobot, symbolic = True)Then the output will be:
# NumPy Array
>>> Jg
array([[-7.55761747e-02, 9.96957218e-02, 1.71533593e-01, -1.73472348e-18],
[ 4.86840507e-01, 1.54765702e-02, 2.66285418e-02, -1.38777878e-17],
[ 0.00000000e+00, 4.92671734e-01, 9.93335155e-02, -2.60208521e-18],
[ 0.00000000e+00, 1.53400671e-01, 1.53400671e-01, 4.90789056e-01],
[ 0.00000000e+00, -9.88164072e-01, -9.88164072e-01, 7.61891398e-02],
[ 1.00000000e+00, 6.12323400e-17, 6.12323400e-17, -8.67940849e-01]])
# SymPy Matrix
>>> symbolicJg
Matrix([[-(L2*cos(q2) + L3*sin(q2 + q3))*sin(q1), (-L2*sin(q2) + L3*cos(q2 + q3))*cos(q1), L3*cos(q1)*cos(q2 + q3), 0],
[ (L2*cos(q2) + L3*sin(q2 + q3))*cos(q1), (-L2*sin(q2) + L3*cos(q2 + q3))*sin(q1), L3*sin(q1)*cos(q2 + q3), 0],
[ 0, L2*cos(q2) + L3*sin(q2 + q3), L3*sin(q2 + q3), 0],
[ 0, sin(q1), sin(q1), sin(q2 + q3)*cos(q1)],
[ 0, -cos(q1), -cos(q1), sin(q1)*sin(q2 + q3)],
[ 1, 0, 0, -cos(q2 + q3)]])
# NumPy Array
>>> Ja
array([[-0.07581958, 0.09945228, 0.17148449, 0. ],
[ 0.48680264, 0.01543878, 0.02662092, 0. ],
[ 0. , 0.4927221 , 0.09942029, 0. ],
[-0.41114626, 0.7160252 , 0.7160252 , 0.72266962],
[-1.34563675, -0.31800502, -0.31800502, 1.20728129],
[ 0.23911895, -1.0821444 , -1.0821444 , 0.23911895]])
# SymPy Matrix
>>> symbolicJa
Matrix([[ -(L2*cos(q2) + L3*sin(q2 + q3))*sin(q1), (-L2*sin(q2) + L3*cos(q2 + q3))*cos(q1), L3*cos(q1)*cos(q2 + q3), 0],
[ (L2*cos(q2) + L3*sin(q2 + q3))*cos(q1), (-L2*sin(q2) + L3*cos(q2 + q3))*sin(q1), L3*sin(q1)*cos(q2 + q3), 0],
[ 0, L2*cos(q2) + L3*sin(q2 + q3), L3*sin(q2 + q3), 0],
[ -sin(q2 + q3)*cos(q1)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2) - (-2*sin(q1 + q4)*cos(q2 + q3) + 2*sin(q1 + q4))*(sin(q1) + sin(q4))*(cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)*sin(q2 + q3)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/(2*(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)**(3/2)) + (-sin(q1 + q4)*cos(q2 + q3)/2 + sin(q1 + q4)/2)*(sin(q1) + sin(q4))*sin(q2 + q3)/(sqrt(1 - (cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)**2)*sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)), -(sin(q1) + sin(q4))*cos(q2 + q3)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2) - (-2*sin(q2 + q3)*cos(q1 + q4) + 2*sin(q2 + q3))*(sin(q1) + sin(q4))*(cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)*sin(q2 + q3)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/(2*(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)**(3/2)) + (-sin(q2 + q3)*cos(q1 + q4)/2 + sin(q2 + q3)/2)*(sin(q1) + sin(q4))*sin(q2 + q3)/(sqrt(1 - (cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)**2)*sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)), -(sin(q1) + sin(q4))*cos(q2 + q3)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2) - (-2*sin(q2 + q3)*cos(q1 + q4) + 2*sin(q2 + q3))*(sin(q1) + sin(q4))*(cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)*sin(q2 + q3)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/(2*(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)**(3/2)) + (-sin(q2 + q3)*cos(q1 + q4)/2 + sin(q2 + q3)/2)*(sin(q1) + sin(q4))*sin(q2 + q3)/(sqrt(1 - (cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)**2)*sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)), -sin(q2 + q3)*cos(q4)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2) - (-2*sin(q1 + q4)*cos(q2 + q3) + 2*sin(q1 + q4))*(sin(q1) + sin(q4))*(cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)*sin(q2 + q3)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/(2*(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)**(3/2)) + (-sin(q1 + q4)*cos(q2 + q3)/2 + sin(q1 + q4)/2)*(sin(q1) + sin(q4))*sin(q2 + q3)/(sqrt(1 - (cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)**2)*sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2))],
[ -sin(q1)*sin(q2 + q3)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2) + (-2*sin(q1 + q4)*cos(q2 + q3) + 2*sin(q1 + q4))*(cos(q1) - cos(q4))*(cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)*sin(q2 + q3)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/(2*(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)**(3/2)) - (-sin(q1 + q4)*cos(q2 + q3)/2 + sin(q1 + q4)/2)*(cos(q1) - cos(q4))*sin(q2 + q3)/(sqrt(1 - (cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)**2)*sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)), (cos(q1) - cos(q4))*cos(q2 + q3)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2) + (-2*sin(q2 + q3)*cos(q1 + q4) + 2*sin(q2 + q3))*(cos(q1) - cos(q4))*(cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)*sin(q2 + q3)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/(2*(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)**(3/2)) - (-sin(q2 + q3)*cos(q1 + q4)/2 + sin(q2 + q3)/2)*(cos(q1) - cos(q4))*sin(q2 + q3)/(sqrt(1 - (cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)**2)*sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)), (cos(q1) - cos(q4))*cos(q2 + q3)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2) + (-2*sin(q2 + q3)*cos(q1 + q4) + 2*sin(q2 + q3))*(cos(q1) - cos(q4))*(cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)*sin(q2 + q3)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/(2*(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)**(3/2)) - (-sin(q2 + q3)*cos(q1 + q4)/2 + sin(q2 + q3)/2)*(cos(q1) - cos(q4))*sin(q2 + q3)/(sqrt(1 - (cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)**2)*sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)), sin(q4)*sin(q2 + q3)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2) + (-2*sin(q1 + q4)*cos(q2 + q3) + 2*sin(q1 + q4))*(cos(q1) - cos(q4))*(cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)*sin(q2 + q3)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/(2*(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)**(3/2)) - (-sin(q1 + q4)*cos(q2 + q3)/2 + sin(q1 + q4)/2)*(cos(q1) - cos(q4))*sin(q2 + q3)/(sqrt(1 - (cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)**2)*sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2))],
[(cos(q2 + q3) - 1)*cos(q1 + q4)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2) + (-2*sin(q1 + q4)*cos(q2 + q3) + 2*sin(q1 + q4))*(cos(q2 + q3) - 1)*(cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)*sin(q1 + q4)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/(2*(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)**(3/2)) - (-sin(q1 + q4)*cos(q2 + q3)/2 + sin(q1 + q4)/2)*(cos(q2 + q3) - 1)*sin(q1 + q4)/(sqrt(1 - (cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)**2)*sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)), -sin(q1 + q4)*sin(q2 + q3)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2) + (-2*sin(q2 + q3)*cos(q1 + q4) + 2*sin(q2 + q3))*(cos(q2 + q3) - 1)*(cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)*sin(q1 + q4)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/(2*(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)**(3/2)) - (-sin(q2 + q3)*cos(q1 + q4)/2 + sin(q2 + q3)/2)*(cos(q2 + q3) - 1)*sin(q1 + q4)/(sqrt(1 - (cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)**2)*sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)), -sin(q1 + q4)*sin(q2 + q3)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2) + (-2*sin(q2 + q3)*cos(q1 + q4) + 2*sin(q2 + q3))*(cos(q2 + q3) - 1)*(cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)*sin(q1 + q4)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/(2*(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)**(3/2)) - (-sin(q2 + q3)*cos(q1 + q4)/2 + sin(q2 + q3)/2)*(cos(q2 + q3) - 1)*sin(q1 + q4)/(sqrt(1 - (cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)**2)*sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)), (cos(q2 + q3) - 1)*cos(q1 + q4)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2) + (-2*sin(q1 + q4)*cos(q2 + q3) + 2*sin(q1 + q4))*(cos(q2 + q3) - 1)*(cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)*sin(q1 + q4)*acos(cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)/(2*(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2)**(3/2)) - (-sin(q1 + q4)*cos(q2 + q3)/2 + sin(q1 + q4)/2)*(cos(q2 + q3) - 1)*sin(q1 + q4)/(sqrt(1 - (cos(q1 + q4)*cos(q2 + q3)/2 - cos(q1 + q4)/2 - cos(q2 + q3)/2 - 1/2)**2)*sqrt(4 - (cos(q1 + q4)*cos(q2 + q3) - cos(q1 + q4) - cos(q2 + q3) - 1)**2))]])
# NumPy Array
>>> Jdq
array([[-0.1080318 , -0.26155557, -0.26155557, -0.1080318 ],
[ 0.19613282, -0.11742141, -0.11742141, -0.19613282],
[ 0.44161581, 0.05214984, 0.05214984, -0.44161581],
[-0.06954512, 0.40630197, 0.40630197, -0.06954512],
[ 0.07135409, -0.10805327, -0.09509771, 0.07135409],
[-0.01503999, 0.03786138, -0.04307636, 0.01503999],
[ 0.00667964, 0.08524929, -0.09699143, -0.00667964],
[-0.11084186, -0.06955894, -0.06121884, -0.11084186]])
# SymPy Matrix
>>> symbolicJdq
Matrix([[ -sin(q2/2 + q3/2)*cos(q1/2 + q4/2)/2, -sin(q1/2 - q2/2 - q3/2 + q4/2)/4 - sin(q1/2 + q2/2 + q3/2 + q4/2)/4, -sin(q1/2 - q2/2 - q3/2 + q4/2)/4 - sin(q1/2 + q2/2 + q3/2 + q4/2)/4, -sin(q2/2 + q3/2)*cos(q1/2 + q4/2)/2],
[ -sin(q1/2 - q4/2)*cos(q2/2 + q3/2)/2, -sin(-q1/2 + q2/2 + q3/2 + q4/2)/4 - sin(q1/2 + q2/2 + q3/2 - q4/2)/4, -sin(-q1/2 + q2/2 + q3/2 + q4/2)/4 - sin(q1/2 + q2/2 + q3/2 - q4/2)/4, sin(q1/2 - q4/2)*cos(q2/2 + q3/2)/2],
[ cos(q1/2 - q4/2)*cos(q2/2 + q3/2)/2, -cos(-q1/2 + q2/2 + q3/2 + q4/2)/4 + cos(q1/2 + q2/2 + q3/2 - q4/2)/4, -cos(-q1/2 + q2/2 + q3/2 + q4/2)/4 + cos(q1/2 + q2/2 + q3/2 - q4/2)/4, -cos(q1/2 - q4/2)*cos(q2/2 + q3/2)/2],
[ -sin(q1/2 + q4/2)*sin(q2/2 + q3/2)/2, cos(q1/2 - q2/2 - q3/2 + q4/2)/4 + cos(q1/2 + q2/2 + q3/2 + q4/2)/4, cos(q1/2 - q2/2 - q3/2 + q4/2)/4 + cos(q1/2 + q2/2 + q3/2 + q4/2)/4, -sin(q1/2 + q4/2)*sin(q2/2 + q3/2)/2],
[ (L1*sin(q2/2 + q3/2) + L2*cos(q2/2 - q3/2) + L3*sin(q2/2 + q3/2))*sin(q1/2 + q4/2)/4, L1*sin(q1/2)*sin(q4/2)*cos(q2/2 + q3/2)/2 - L1*cos(q1/2 - q4/2)*cos(q2/2 + q3/2)/4 - L2*sin(q1/2)*sin(q4/2)*sin(q2/2 - q3/2)/2 + L2*sin(q2/2 - q3/2)*cos(q1/2 - q4/2)/4 + L3*sin(q1/2)*sin(q4/2)*cos(q2/2 + q3/2)/2 - L3*cos(q1/2 - q4/2)*cos(q2/2 + q3/2)/4, L1*sin(q2/2)**3*sin(q3/2)*cos(q1/2 + q4/2)/4 + L1*sin(q2/2)**2*cos(q2/2)*cos(q3/2)*cos(q1/2 + q4/2)/4 - L1*sin(q2/2)*sin(q2/2 - q3/2)*cos(q2/2)*cos(q1/2 + q4/2)/4 - L1*cos(q2/2)*cos(q3/2)*cos(q1/2 + q4/2)/4 - L2*sin(q2/2)**3*cos(q3/2)*cos(q1/2 + q4/2)/4 - L2*sin(q2/2)**2*sin(q3/2)*cos(q2/2)*cos(q1/2 + q4/2)/4 - L2*sin(q2/2)*cos(q2/2)*cos(q1/2 + q4/2)*cos(q2/2 + q3/2)/4 + L2*sin(q3/2)*cos(q2/2)*cos(q1/2 + q4/2)/4 + L3*sin(q2/2)**3*sin(q3/2)*cos(q1/2 + q4/2)/4 + L3*sin(q2/2)**2*cos(q2/2)*cos(q3/2)*cos(q1/2 + q4/2)/4 - L3*sin(q2/2)*sin(q2/2 - q3/2)*cos(q2/2)*cos(q1/2 + q4/2)/4 - L3*cos(q2/2)*cos(q3/2)*cos(q1/2 + q4/2)/4, L1*cos(q1/2 - q2/2 - q3/2 + q4/2)/8 - L1*cos(q1/2 + q2/2 + q3/2 + q4/2)/8 + L2*sin(q1/2 - q2/2 + q3/2 + q4/2)/8 + L2*sin(q1/2 + q2/2 - q3/2 + q4/2)/8 + L3*cos(q1/2 - q2/2 - q3/2 + q4/2)/8 - L3*cos(q1/2 + q2/2 + q3/2 + q4/2)/8],
[(-L1*cos(q2/2 + q3/2) - L2*sin(q2/2 - q3/2) + L3*cos(q2/2 + q3/2))*cos(q1/2 - q4/2)/4, L1*sin(q1/2)*sin(q2/2 + q3/2)*cos(q4/2)/2 - L1*sin(q1/2 + q4/2)*sin(q2/2 + q3/2)/4 - L2*sin(q1/2)*cos(q4/2)*cos(q2/2 - q3/2)/2 + L2*sin(q1/2 + q4/2)*cos(q2/2 - q3/2)/4 - L3*sin(q1/2)*sin(q2/2 + q3/2)*cos(q4/2)/2 + L3*sin(q1/2 + q4/2)*sin(q2/2 + q3/2)/4, L1*sin(q1/2)**2*sin(q2/2)**2*sin(q1/2 - q4/2)*sin(q2/2 + q3/2)/4 + L1*sin(q1/2)**2*sin(q1/2 - q4/2)*sin(q2/2 + q3/2)*cos(q2/2)**2/4 + L1*sin(q2/2)**3*sin(q1/2 - q4/2)*cos(q1/2)**2*cos(q3/2)/4 + L1*sin(q2/2)*sin(q1/2 - q4/2)*cos(q1/2)**2*cos(q2/2)*cos(q2/2 - q3/2)/4 + L1*sin(q3/2)*sin(q1/2 - q4/2)*cos(q1/2)**2*cos(q2/2)**3/4 + L2*sin(q1/2)**2*sin(q2/2)**2*sin(q1/2 - q4/2)*cos(q2/2 - q3/2)/4 + L2*sin(q1/2)**2*sin(q1/2 - q4/2)*cos(q2/2)**2*cos(q2/2 - q3/2)/4 + L2*sin(q2/2)**3*sin(q3/2)*sin(q1/2 - q4/2)*cos(q1/2)**2/4 + L2*sin(q2/2)*sin(q1/2 - q4/2)*sin(q2/2 + q3/2)*cos(q1/2)**2*cos(q2/2)/4 + L2*sin(q1/2 - q4/2)*cos(q1/2)**2*cos(q2/2)**3*cos(q3/2)/4 - L3*sin(q1/2)**2*sin(q2/2)**3*sin(q1/2 - q4/2)*cos(q3/2)/4 - L3*sin(q1/2)**2*sin(q2/2)*sin(q1/2 - q4/2)*cos(q2/2)*cos(q2/2 - q3/2)/4 - L3*sin(q1/2)**2*sin(q3/2)*sin(q1/2 - q4/2)*cos(q2/2)**3/4 - L3*sin(q2/2)**2*sin(q1/2 - q4/2)*sin(q2/2 + q3/2)*cos(q1/2)**2/4 - L3*sin(q1/2 - q4/2)*sin(q2/2 + q3/2)*cos(q1/2)**2*cos(q2/2)**2/4, L1*cos(-q1/2 + q2/2 + q3/2 + q4/2)/8 + L1*cos(q1/2 + q2/2 + q3/2 - q4/2)/8 - L2*sin(q1/2 - q2/2 + q3/2 - q4/2)/8 + L2*sin(q1/2 + q2/2 - q3/2 - q4/2)/8 - L3*cos(-q1/2 + q2/2 + q3/2 + q4/2)/8 - L3*cos(q1/2 + q2/2 + q3/2 - q4/2)/8],
[(-L1*cos(q2/2 + q3/2) - L2*sin(q2/2 - q3/2) + L3*cos(q2/2 + q3/2))*sin(q1/2 - q4/2)/4, -L1*sin(q1/2)*sin(q4/2)*sin(q2/2 + q3/2)/2 - L1*sin(q2/2 + q3/2)*cos(q1/2 + q4/2)/4 + L2*sin(q1/2)*sin(q4/2)*cos(q2/2 - q3/2)/2 + L2*cos(q1/2 + q4/2)*cos(q2/2 - q3/2)/4 + L3*sin(q1/2)*sin(q4/2)*sin(q2/2 + q3/2)/2 + L3*sin(q2/2 + q3/2)*cos(q1/2 + q4/2)/4, -L1*sin(q2/2)**3*cos(q3/2)*cos(q1/2 - q4/2)/4 + L1*sin(q2/2)**2*sin(q3/2)*cos(q2/2)*cos(q1/2 - q4/2)/4 - L1*sin(q2/2)*cos(q2/2)*cos(q1/2 - q4/2)*cos(q2/2 - q3/2)/4 - L1*sin(q3/2)*cos(q2/2)*cos(q1/2 - q4/2)/4 - L2*sin(q2/2)**3*sin(q3/2)*cos(q1/2 - q4/2)/4 + L2*sin(q2/2)**2*cos(q2/2)*cos(q3/2)*cos(q1/2 - q4/2)/4 - L2*sin(q2/2)*sin(q2/2 + q3/2)*cos(q2/2)*cos(q1/2 - q4/2)/4 - L2*cos(q2/2)*cos(q3/2)*cos(q1/2 - q4/2)/4 + L3*sin(q2/2)**3*cos(q3/2)*cos(q1/2 - q4/2)/4 - L3*sin(q2/2)**2*sin(q3/2)*cos(q2/2)*cos(q1/2 - q4/2)/4 + L3*sin(q2/2)*cos(q2/2)*cos(q1/2 - q4/2)*cos(q2/2 - q3/2)/4 + L3*sin(q3/2)*cos(q2/2)*cos(q1/2 - q4/2)/4, -L1*sin(-q1/2 + q2/2 + q3/2 + q4/2)/8 + L1*sin(q1/2 + q2/2 + q3/2 - q4/2)/8 + L2*cos(q1/2 - q2/2 + q3/2 - q4/2)/8 - L2*cos(q1/2 + q2/2 - q3/2 - q4/2)/8 + L3*sin(-q1/2 + q2/2 + q3/2 + q4/2)/8 - L3*sin(q1/2 + q2/2 + q3/2 - q4/2)/8],
[-(L1*sin(q2/2 + q3/2) + L2*cos(q2/2 - q3/2) + L3*sin(q2/2 + q3/2))*cos(q1/2 + q4/2)/4, -L1*sin(q1/2)*cos(q4/2)*cos(q2/2 + q3/2)/2 + L1*sin(q1/2 - q4/2)*cos(q2/2 + q3/2)/4 + L2*sin(q1/2)*sin(q2/2 - q3/2)*cos(q4/2)/2 - L2*sin(q1/2 - q4/2)*sin(q2/2 - q3/2)/4 - L3*sin(q1/2)*cos(q4/2)*cos(q2/2 + q3/2)/2 + L3*sin(q1/2 - q4/2)*cos(q2/2 + q3/2)/4, -L1*sin(q1/2)**3*cos(q2/2)**2*cos(q4/2)*cos(q2/2 + q3/2)/4 - L1*sin(q1/2)**2*sin(q2/2)**2*sin(q1/2 + q4/2)*cos(q2/2 + q3/2)/4 - L1*sin(q1/2)*cos(q1/2)*cos(q2/2)**2*cos(q1/2 - q4/2)*cos(q2/2 + q3/2)/4 - L1*sin(q2/2)**2*sin(q1/2 + q4/2)*cos(q1/2)**2*cos(q2/2 + q3/2)/4 - L1*sin(q4/2)*cos(q1/2)**3*cos(q2/2)**2*cos(q2/2 + q3/2)/4 - L2*sin(q1/2)**3*sin(q2/2)**2*sin(q2/2 - q3/2)*cos(q4/2)/4 - L2*sin(q1/2)**2*sin(q1/2 + q4/2)*sin(q2/2 - q3/2)*cos(q2/2)**2/4 - L2*sin(q1/2)*sin(q2/2)**2*sin(q2/2 - q3/2)*cos(q1/2)*cos(q1/2 - q4/2)/4 - L2*sin(q2/2)**2*sin(q4/2)*sin(q2/2 - q3/2)*cos(q1/2)**3/4 - L2*sin(q1/2 + q4/2)*sin(q2/2 - q3/2)*cos(q1/2)**2*cos(q2/2)**2/4 - L3*sin(q1/2)**3*cos(q2/2)**2*cos(q4/2)*cos(q2/2 + q3/2)/4 - L3*sin(q1/2)**2*sin(q2/2)**2*sin(q1/2 + q4/2)*cos(q2/2 + q3/2)/4 - L3*sin(q1/2)*cos(q1/2)*cos(q2/2)**2*cos(q1/2 - q4/2)*cos(q2/2 + q3/2)/4 - L3*sin(q2/2)**2*sin(q1/2 + q4/2)*cos(q1/2)**2*cos(q2/2 + q3/2)/4 - L3*sin(q4/2)*cos(q1/2)**3*cos(q2/2)**2*cos(q2/2 + q3/2)/4, L1*sin(q1/2 - q2/2 - q3/2 + q4/2)/8 - L1*sin(q1/2 + q2/2 + q3/2 + q4/2)/8 - L2*cos(q1/2 - q2/2 + q3/2 + q4/2)/8 - L2*cos(q1/2 + q2/2 - q3/2 + q4/2)/8 + L3*sin(q1/2 - q2/2 - q3/2 + q4/2)/8 - L3*sin(q1/2 + q2/2 + q3/2 + q4/2)/8]]) IMPORTANT NOTE: Please notice that symbolic computation is slower than numerical one, so use those commands only if you need to know the equations of your system. Take a look at Symbolic Computation for more information 😉
This is OPTIONAL. If you need to calculate the end-effector acceleration, you will need the derivative of a Jacobian Matrix:
,
where is the vector of generalized coordinates of the system, however, calculating this leads to differentiate every single term in the jacobian matrix (that's a really hard task). To solve it, a Geometric solution to this problem is described with the following equation:
Because of the size of the equation, this was shortened to a single column to exemplify how this is calculated. In this case, j represents the j - th joint and column of the matrix, so its dimensions are . Then, the derivative can be calculated with the library as follows
# Kinematics libraries
from lib.kinematics.HTM import *
# Derivative of Geometric Jacobian Matrix (OPTIONAL)
dJg = geometricJacobianDerivative(uRobot, symbolic = False)Then, the output will be
# NumPy Array
>>> dJg
array([[ 1.15393988, 0.31198438, -0.17393471, -0.28141928],
[ 0.01563244, 0.98610127, -1.0573929 , -0.21637846],
[ 0.32384597, 0.17142045, -0.09869656, 0.54228632],
[-0.66229389, 0.6116456 , 0.6116456 , 0. ],
[-1.17576549, 0.00828597, 0.00828597, 0. ],
[ 0. , -1.16668634, -1.16668634, 0. ]])You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
This is OPTIONAL. If you need to calculate the acceleration of any center of mass (COM), you will need the derivative of a Jacobian Matrix:
,
where is the vector of generalized coordinates of the system, however, calculating this leads to differentiate every single term in the jacobian matrix of the j - th center of mass (that's a really hard task). To solve it, a Geometric solution to this problem is described with the following equation:
Because of the size of the equation, this was shortened to a single column to exemplify how this is calculated. In this case, j represents the j - th joint and column of the matrix, so its dimensions are . Then, the derivative can be calculated with the library as follows
# Kinematics libraries
from lib.kinematics.HTM import *
# Derivative of Geometric Jacobian Matrix to any acenter of mass (OPTIONAL)
dJgCOM = geometricJacobianDerivativeCOM(uRobot, COM = 2, symbolic = False)Then, the output will be
# NumPy Array
>>> dJgCOM
array([[ 5.89677598e-02 -1.60894524e-01 0.00000000e+00 0.00000000e+00]
[ 6.07744491e-02 1.00827098e-01 0.00000000e+00 0.00000000e+00]
[-7.05724225e-18 -4.35017413e-02 0.00000000e+00 0.00000000e+00]
[ 1.77058883e-01 0.00000000e+00 0.00000000e+00 0.00000000e+00]
[ 1.82483718e-01 0.00000000e+00 0.00000000e+00 0.00000000e+00]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00]])You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
This is OPTIONAL, because each function that needs a Dual Inertial Jacobian Matrix can call and process it automatically 😉 but you can calculate its geometrical or analytical form as follows:
# Kinematics libraries
from lib.kinematics.DQ import *
# Dual Jacobian Matrix (OPTIONAL)
Jdq = jacobianDQ(uRobot)
symbolicJdq = jacobianDQ(uRobot, symbolic = True)
# Dual Velocity Jacobian Matrix (OPTIONAL)
Jvdq = jacobianVelocityDQ(uRobot)
symbolicJvdq = jacobianVelocityDQ(uRobot, symbolic = True)Then the output will be:
# NumPy Array
>>> Jdq
array([[-0.1080318 , -0.26155557, -0.26155557, -0.1080318 ],
[ 0.19613282, -0.11742141, -0.11742141, -0.19613282],
[ 0.44161581, 0.05214984, 0.05214984, -0.44161581],
[-0.06954512, 0.40630197, 0.40630197, -0.06954512],
[ 0.07135409, -0.10805327, -0.09509771, 0.07135409],
[-0.01503999, 0.03786138, -0.04307636, 0.01503999],
[ 0.00667964, 0.08524929, -0.09699143, -0.00667964],
[-0.11084186, -0.06955894, -0.06121884, -0.11084186]])
# SymPy Matrix
>>> symbolicJdq
Matrix([[ -sin(q2/2 + q3/2)*cos(q1/2 + q4/2)/2, -sin(q1/2 - q2/2 - q3/2 + q4/2)/4 - sin(q1/2 + q2/2 + q3/2 + q4/2)/4, -sin(q1/2 - q2/2 - q3/2 + q4/2)/4 - sin(q1/2 + q2/2 + q3/2 + q4/2)/4, -sin(q2/2 + q3/2)*cos(q1/2 + q4/2)/2],
[ -sin(q1/2 - q4/2)*cos(q2/2 + q3/2)/2, -sin(-q1/2 + q2/2 + q3/2 + q4/2)/4 - sin(q1/2 + q2/2 + q3/2 - q4/2)/4, -sin(-q1/2 + q2/2 + q3/2 + q4/2)/4 - sin(q1/2 + q2/2 + q3/2 - q4/2)/4, sin(q1/2 - q4/2)*cos(q2/2 + q3/2)/2],
[ cos(q1/2 - q4/2)*cos(q2/2 + q3/2)/2, -cos(-q1/2 + q2/2 + q3/2 + q4/2)/4 + cos(q1/2 + q2/2 + q3/2 - q4/2)/4, -cos(-q1/2 + q2/2 + q3/2 + q4/2)/4 + cos(q1/2 + q2/2 + q3/2 - q4/2)/4, -cos(q1/2 - q4/2)*cos(q2/2 + q3/2)/2],
[ -sin(q1/2 + q4/2)*sin(q2/2 + q3/2)/2, cos(q1/2 - q2/2 - q3/2 + q4/2)/4 + cos(q1/2 + q2/2 + q3/2 + q4/2)/4, cos(q1/2 - q2/2 - q3/2 + q4/2)/4 + cos(q1/2 + q2/2 + q3/2 + q4/2)/4, -sin(q1/2 + q4/2)*sin(q2/2 + q3/2)/2],
[ (L1*sin(q2/2 + q3/2) + L2*cos(q2/2 - q3/2) + L3*sin(q2/2 + q3/2))*sin(q1/2 + q4/2)/4, L1*sin(q1/2)*sin(q4/2)*cos(q2/2 + q3/2)/2 - L1*cos(q1/2 - q4/2)*cos(q2/2 + q3/2)/4 - L2*sin(q1/2)*sin(q4/2)*sin(q2/2 - q3/2)/2 + L2*sin(q2/2 - q3/2)*cos(q1/2 - q4/2)/4 + L3*sin(q1/2)*sin(q4/2)*cos(q2/2 + q3/2)/2 - L3*cos(q1/2 - q4/2)*cos(q2/2 + q3/2)/4, L1*sin(q2/2)**3*sin(q3/2)*cos(q1/2 + q4/2)/4 + L1*sin(q2/2)**2*cos(q2/2)*cos(q3/2)*cos(q1/2 + q4/2)/4 - L1*sin(q2/2)*sin(q2/2 - q3/2)*cos(q2/2)*cos(q1/2 + q4/2)/4 - L1*cos(q2/2)*cos(q3/2)*cos(q1/2 + q4/2)/4 - L2*sin(q2/2)**3*cos(q3/2)*cos(q1/2 + q4/2)/4 - L2*sin(q2/2)**2*sin(q3/2)*cos(q2/2)*cos(q1/2 + q4/2)/4 - L2*sin(q2/2)*cos(q2/2)*cos(q1/2 + q4/2)*cos(q2/2 + q3/2)/4 + L2*sin(q3/2)*cos(q2/2)*cos(q1/2 + q4/2)/4 + L3*sin(q2/2)**3*sin(q3/2)*cos(q1/2 + q4/2)/4 + L3*sin(q2/2)**2*cos(q2/2)*cos(q3/2)*cos(q1/2 + q4/2)/4 - L3*sin(q2/2)*sin(q2/2 - q3/2)*cos(q2/2)*cos(q1/2 + q4/2)/4 - L3*cos(q2/2)*cos(q3/2)*cos(q1/2 + q4/2)/4, L1*cos(q1/2 - q2/2 - q3/2 + q4/2)/8 - L1*cos(q1/2 + q2/2 + q3/2 + q4/2)/8 + L2*sin(q1/2 - q2/2 + q3/2 + q4/2)/8 + L2*sin(q1/2 + q2/2 - q3/2 + q4/2)/8 + L3*cos(q1/2 - q2/2 - q3/2 + q4/2)/8 - L3*cos(q1/2 + q2/2 + q3/2 + q4/2)/8],
[(-L1*cos(q2/2 + q3/2) - L2*sin(q2/2 - q3/2) + L3*cos(q2/2 + q3/2))*cos(q1/2 - q4/2)/4, L1*sin(q1/2)*sin(q2/2 + q3/2)*cos(q4/2)/2 - L1*sin(q1/2 + q4/2)*sin(q2/2 + q3/2)/4 - L2*sin(q1/2)*cos(q4/2)*cos(q2/2 - q3/2)/2 + L2*sin(q1/2 + q4/2)*cos(q2/2 - q3/2)/4 - L3*sin(q1/2)*sin(q2/2 + q3/2)*cos(q4/2)/2 + L3*sin(q1/2 + q4/2)*sin(q2/2 + q3/2)/4, L1*sin(q1/2)**2*sin(q2/2)**2*sin(q1/2 - q4/2)*sin(q2/2 + q3/2)/4 + L1*sin(q1/2)**2*sin(q1/2 - q4/2)*sin(q2/2 + q3/2)*cos(q2/2)**2/4 + L1*sin(q2/2)**3*sin(q1/2 - q4/2)*cos(q1/2)**2*cos(q3/2)/4 + L1*sin(q2/2)*sin(q1/2 - q4/2)*cos(q1/2)**2*cos(q2/2)*cos(q2/2 - q3/2)/4 + L1*sin(q3/2)*sin(q1/2 - q4/2)*cos(q1/2)**2*cos(q2/2)**3/4 + L2*sin(q1/2)**2*sin(q2/2)**2*sin(q1/2 - q4/2)*cos(q2/2 - q3/2)/4 + L2*sin(q1/2)**2*sin(q1/2 - q4/2)*cos(q2/2)**2*cos(q2/2 - q3/2)/4 + L2*sin(q2/2)**3*sin(q3/2)*sin(q1/2 - q4/2)*cos(q1/2)**2/4 + L2*sin(q2/2)*sin(q1/2 - q4/2)*sin(q2/2 + q3/2)*cos(q1/2)**2*cos(q2/2)/4 + L2*sin(q1/2 - q4/2)*cos(q1/2)**2*cos(q2/2)**3*cos(q3/2)/4 - L3*sin(q1/2)**2*sin(q2/2)**3*sin(q1/2 - q4/2)*cos(q3/2)/4 - L3*sin(q1/2)**2*sin(q2/2)*sin(q1/2 - q4/2)*cos(q2/2)*cos(q2/2 - q3/2)/4 - L3*sin(q1/2)**2*sin(q3/2)*sin(q1/2 - q4/2)*cos(q2/2)**3/4 - L3*sin(q2/2)**2*sin(q1/2 - q4/2)*sin(q2/2 + q3/2)*cos(q1/2)**2/4 - L3*sin(q1/2 - q4/2)*sin(q2/2 + q3/2)*cos(q1/2)**2*cos(q2/2)**2/4, L1*cos(-q1/2 + q2/2 + q3/2 + q4/2)/8 + L1*cos(q1/2 + q2/2 + q3/2 - q4/2)/8 - L2*sin(q1/2 - q2/2 + q3/2 - q4/2)/8 + L2*sin(q1/2 + q2/2 - q3/2 - q4/2)/8 - L3*cos(-q1/2 + q2/2 + q3/2 + q4/2)/8 - L3*cos(q1/2 + q2/2 + q3/2 - q4/2)/8],
[(-L1*cos(q2/2 + q3/2) - L2*sin(q2/2 - q3/2) + L3*cos(q2/2 + q3/2))*sin(q1/2 - q4/2)/4, -L1*sin(q1/2)*sin(q4/2)*sin(q2/2 + q3/2)/2 - L1*sin(q2/2 + q3/2)*cos(q1/2 + q4/2)/4 + L2*sin(q1/2)*sin(q4/2)*cos(q2/2 - q3/2)/2 + L2*cos(q1/2 + q4/2)*cos(q2/2 - q3/2)/4 + L3*sin(q1/2)*sin(q4/2)*sin(q2/2 + q3/2)/2 + L3*sin(q2/2 + q3/2)*cos(q1/2 + q4/2)/4, -L1*sin(q2/2)**3*cos(q3/2)*cos(q1/2 - q4/2)/4 + L1*sin(q2/2)**2*sin(q3/2)*cos(q2/2)*cos(q1/2 - q4/2)/4 - L1*sin(q2/2)*cos(q2/2)*cos(q1/2 - q4/2)*cos(q2/2 - q3/2)/4 - L1*sin(q3/2)*cos(q2/2)*cos(q1/2 - q4/2)/4 - L2*sin(q2/2)**3*sin(q3/2)*cos(q1/2 - q4/2)/4 + L2*sin(q2/2)**2*cos(q2/2)*cos(q3/2)*cos(q1/2 - q4/2)/4 - L2*sin(q2/2)*sin(q2/2 + q3/2)*cos(q2/2)*cos(q1/2 - q4/2)/4 - L2*cos(q2/2)*cos(q3/2)*cos(q1/2 - q4/2)/4 + L3*sin(q2/2)**3*cos(q3/2)*cos(q1/2 - q4/2)/4 - L3*sin(q2/2)**2*sin(q3/2)*cos(q2/2)*cos(q1/2 - q4/2)/4 + L3*sin(q2/2)*cos(q2/2)*cos(q1/2 - q4/2)*cos(q2/2 - q3/2)/4 + L3*sin(q3/2)*cos(q2/2)*cos(q1/2 - q4/2)/4, -L1*sin(-q1/2 + q2/2 + q3/2 + q4/2)/8 + L1*sin(q1/2 + q2/2 + q3/2 - q4/2)/8 + L2*cos(q1/2 - q2/2 + q3/2 - q4/2)/8 - L2*cos(q1/2 + q2/2 - q3/2 - q4/2)/8 + L3*sin(-q1/2 + q2/2 + q3/2 + q4/2)/8 - L3*sin(q1/2 + q2/2 + q3/2 - q4/2)/8],
[-(L1*sin(q2/2 + q3/2) + L2*cos(q2/2 - q3/2) + L3*sin(q2/2 + q3/2))*cos(q1/2 + q4/2)/4, -L1*sin(q1/2)*cos(q4/2)*cos(q2/2 + q3/2)/2 + L1*sin(q1/2 - q4/2)*cos(q2/2 + q3/2)/4 + L2*sin(q1/2)*sin(q2/2 - q3/2)*cos(q4/2)/2 - L2*sin(q1/2 - q4/2)*sin(q2/2 - q3/2)/4 - L3*sin(q1/2)*cos(q4/2)*cos(q2/2 + q3/2)/2 + L3*sin(q1/2 - q4/2)*cos(q2/2 + q3/2)/4, -L1*sin(q1/2)**3*cos(q2/2)**2*cos(q4/2)*cos(q2/2 + q3/2)/4 - L1*sin(q1/2)**2*sin(q2/2)**2*sin(q1/2 + q4/2)*cos(q2/2 + q3/2)/4 - L1*sin(q1/2)*cos(q1/2)*cos(q2/2)**2*cos(q1/2 - q4/2)*cos(q2/2 + q3/2)/4 - L1*sin(q2/2)**2*sin(q1/2 + q4/2)*cos(q1/2)**2*cos(q2/2 + q3/2)/4 - L1*sin(q4/2)*cos(q1/2)**3*cos(q2/2)**2*cos(q2/2 + q3/2)/4 - L2*sin(q1/2)**3*sin(q2/2)**2*sin(q2/2 - q3/2)*cos(q4/2)/4 - L2*sin(q1/2)**2*sin(q1/2 + q4/2)*sin(q2/2 - q3/2)*cos(q2/2)**2/4 - L2*sin(q1/2)*sin(q2/2)**2*sin(q2/2 - q3/2)*cos(q1/2)*cos(q1/2 - q4/2)/4 - L2*sin(q2/2)**2*sin(q4/2)*sin(q2/2 - q3/2)*cos(q1/2)**3/4 - L2*sin(q1/2 + q4/2)*sin(q2/2 - q3/2)*cos(q1/2)**2*cos(q2/2)**2/4 - L3*sin(q1/2)**3*cos(q2/2)**2*cos(q4/2)*cos(q2/2 + q3/2)/4 - L3*sin(q1/2)**2*sin(q2/2)**2*sin(q1/2 + q4/2)*cos(q2/2 + q3/2)/4 - L3*sin(q1/2)*cos(q1/2)*cos(q2/2)**2*cos(q1/2 - q4/2)*cos(q2/2 + q3/2)/4 - L3*sin(q2/2)**2*sin(q1/2 + q4/2)*cos(q1/2)**2*cos(q2/2 + q3/2)/4 - L3*sin(q4/2)*cos(q1/2)**3*cos(q2/2)**2*cos(q2/2 + q3/2)/4, L1*sin(q1/2 - q2/2 - q3/2 + q4/2)/8 - L1*sin(q1/2 + q2/2 + q3/2 + q4/2)/8 - L2*cos(q1/2 - q2/2 + q3/2 + q4/2)/8 - L2*cos(q1/2 + q2/2 - q3/2 + q4/2)/8 + L3*sin(q1/2 - q2/2 - q3/2 + q4/2)/8 - L3*sin(q1/2 + q2/2 + q3/2 + q4/2)/8]])
# NumPy Array
>>> Jvdq
array([[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 -1.73472348e-18]
[ 0.00000000e+00 -1.31343750e-01 -1.31343750e-01 3.38454149e-01]
[ 0.00000000e+00 -9.91336885e-01 -9.91336885e-01 -4.48423113e-02]
[ 1.00000000e+00 0.00000000e+00 0.00000000e+00 -9.39913802e-01]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 0.00000000e+00]
[ 7.04338427e-03 2.02542641e-02 9.15924166e-02 -2.77555756e-17]
[ 5.31610116e-02 -2.68351863e-03 -1.21352203e-02 -1.38777878e-16]
[ 0.00000000e+00 5.36255761e-02 3.35605294e-02 2.16690945e-17]])
>>> symbolicJvdq
Matrix([
[ 0, 0, 0, 0],
[ 0, sin(q1), sin(q1), sin(q2 + q3)*cos(q1)],
[ 0, -cos(q1), -cos(q1), sin(q1)*sin(q2 + q3)],
[ 1, 0, 0, -cos(q2 + q3)],
[ 0, 0, 0, 0],
[-(L2*cos(q2) + L3*sin(q2 + q3))*sin(q1), (-L2*sin(q2) + L3*cos(q2 + q3))*cos(q1), L3*cos(q1)*cos(q2 + q3), L3*(-sin(q2)*cos(q2) - sin(q3)*cos(q3) + sin(q2 + q3)*cos(q2 - q3))*sin(q1)],
[ (L2*cos(q2) + L3*sin(q2 + q3))*cos(q1), (-L2*sin(q2) + L3*cos(q2 + q3))*sin(q1), L3*sin(q1)*cos(q2 + q3), L3*(sin(q2)*cos(q2) + sin(q3)*cos(q3) - sin(q2 + q3)*cos(q2 - q3))*cos(q1)],
[ 0, L2*cos(q2) + L3*sin(q2 + q3), L3*sin(q2 + q3), 0]])IMPORTANT NOTE: Dual Inertal Jacobian Matrix (Jdq) and Dual Inertial Velocity Jacobian Matrix (Jvdq) are not used for the same purposes, so please take a look at our online workshop (in spanish) on robotics with dual quaternions. Aso, please notice that symbolic computation is slower than numerical one, so use those commands only if you need to know the equations of your system. Take a look at Symbolic Computation for more information 😉
This is OPTIONAL. If you need to calculate the end-effector acceleration, you will need the derivative of a Jacobian Matrix:
,
where is the vector of generalized coordinates of the system, however, calculating this leads to differentiate every single term in the jacobian matrix (that's a really hard task). To solve it, a geometric solution to this problem is described with the following equation:
where represents the adjoint transformation of a dual quaternion. Also, j represents the j - th joint and column of the matrix, so its dimensions are
. Then, the derivative can be calculated with the library as follows
# Kinematics libraries
from lib.kinematics.DQ import *
# Derivative of Dual Inertial Velocity Jacobian Matrix (OPTIONAL)
dJvdq = jacobianVelocityDerivativeDQ(uRobot, symbolic = False)Then, the output will be
# NumPy Array
>>> dJvdq
array([[ 0. , 0. , 0. , 0. ],
[ 0. , -0.29528491, -0.29528491, -0.8325105 ],
[ 0. , 0.23473862, 0.23473862, 1.05237222],
[ 0. , 0. , 0. , 1.80776869],
[ 0. , 0. , 0. , 0. ],
[-0.49616381, -1.01354893, -0.97080722, 0. ],
[-0.70609507, 0.43024639, 0.59524827, 0. ],
[ 0. , -0.86148002, -0.81695769, 0. ]])You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
This is OPTIONAL. If you need to calculate the end-effector acceleration, you will need the derivative of a Jacobian Matrix:
,
where is the vector of generalized coordinates of the system, however, calculating this leads to differentiate every single term in the jacobian matrix (that's a really hard task). To solve it, a geometric solution to this problem is described with the following equation:
where represents the adjoint transformation of a dual quaternion. Also, j represents the j - th joint and column of the matrix, so its dimensions are
. Then, the derivative can be calculated with the library as follows
# Kinematics libraries
from lib.kinematics.DQ import *
# Derivative of Dual Inertial Velocity Jacobian Matrix (OPTIONAL)
dJvdqCOM = jacobianVelocityDerivativeDQCOM(uRobot, COM = 2, symbolic = False)Then, the output will be
# NumPy Array
>>> dJvdqCOM
array([[ 0. , 0. , 0. , 0. ],
[ 0. , 1.32888391, 0. , 0. ],
[ 0. , -1.22064417, 0. , 0. ],
[ 0. , 0. , 0. , 0. ],
[ 0. , 0. , 0. , 0. ],
[-0.62079983, -0.5774601 , 0. , 0. ],
[ 0.2066157 , -0.17156286, 0. , 0. ],
[ 0. , -0.26779209, 0. , 0. ]])You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
Instead of calculating horrible and complex equations, we use a numeiical algorithm to calculate inverse kinematics. It is asymptotically stable only if the pose to be reached is inside the robot's workspace, i.e. the point to be reached is reachable
# Kinematics libraries
from lib.kinematics.HTM import *
from lib.kinematics.DQ import *
# Calculate robot's Inverse Kinematics to a single point (using Homogeneous Transformation Matrices)
qHTM = inverseHTM(uRobot, q0 = np.random.rand(4, 1), Hd = fkHTM[-1], K = 50 * np.eye(6))
# Calculate robot's Inverse Kinematics to a single point (Dual Quaternions)
qDQ = inverseDQ(uRobot, q0 = np.random.rand(4, 1), Qd = fkDQ[-1], K = 50 * np.eye(8), xi = xi) is the initial condition of the generalized coordinates; also,
and
represent the desired frame's pose using an Homogeneous Transformation Matrix or a Dual Quaternion respectively. Last but not least,
and
are the constant gain matrices that are used to solve inverse kinematics problem
IMPORTANT NOTE: Inverse kinematics algorithms returns a generalized coordinates vector , where
is the number of joints positions that will be reached
This is the relation between motion (velocity) in joint space and motion (linear/angular velocity) in task space (e.g., Cartesian space), but it is known that time derivative of end-effector pose won't lead to its inertial velocity
The rate of change of the end-effector (is not the same as its velocity) can be calculated by deriving its equations that define its pose in an Axis - Angle vector , so this will return an Analytic Jacobian Matrix
that can be multiplied by the generalized coordinates vector:
This calculation can be done as follows:
"""
Inertial Rate of Change
"""
# Differential Kinematics library
from lib.kinematics.DifferentialHTM import *
# NumPy
import numpy as np
# Analytic Jacobian Matrix (dq: step size of numerical derivative)
Ja = analyticJacobian(uRobot, dq = 0.001, symbolic = False)
# Inertial Rate of Change (calculated by analytic jacobian and joints velocities)
Xd1 = Ja.dot(uRobot.jointsVelocities)
# Inertial Rate of Change (calculated by library's module)
Xd2 = analyticStateSpace(uRobot, dq = 0.001, symbolic = False)So the outputs will be
# NumPy Array
>>> Ja
array([[ 0.182977 , -0.38393173, -0.62026731, 0. ],
[ 0.28277371, 0.24870669, 0.40180224, 0. ],
[ 0. , 0.33658218, -0.51224527, 0. ],
[ 0.04369075, 1.20061659, 1.20061659, -0.48314347],
[ 0.52885716, -0.84524269, -0.84524269, -0.21947929],
[ 0.91602413, 0.4473779 , 0.4473779 , 0.91602413]])
>>> Xd1
array([[-0.63617328],
[ 0.36004356],
[-1.61991614],
[ 0.67665821],
[-0.22858399],
[-0.49209642]])
>>> Xd2
array([[-0.63617328],
[ 0.36004356],
[-1.61991614],
[ 0.67665821],
[-0.22858399],
[-0.49209642]])You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
End-effector velocity can be calculated with Inertial Geometric Jacobian Matrix, because this maps the effect of each joint directly to the end-effector, so linear and angular velocities can be calculated:
,
This can be calculated with the library as follows:
"""
Total Inertial Velocity (using Homogeneous Transformation Matrices)
"""
# Differential Kinematics library
from lib.kinematics.DifferentialHTM import *
# NumPy
import numpy as np
# Geometric Jacobian Matrix
Jg = geometricJacobian(uRobot, symbolic = False)
# Total Inertial Velocity (calculated by geometric jacobian and joints velocities)
Xd1 = Jg.dot(uRobot.jointsVelocities)
# Total Inertial Velocity (calculated by library's module)
Xd2 = geometricStateSpace(uRobot, symbolic = False)So the outputs will be
# NumPy Array
>>> Jg
array([[ 0.18311838, -0.38379045, -0.62048222, 0. ],
[ 0.2826822 , 0.24861517, 0.40194146, 0. ],
[ 0. , 0.33681088, -0.51187571, 0. ],
[ 0. , -0.54368308, -0.54368308, -0.47776856],
[ 0. , -0.8392906 , -0.8392906 , 0.30949314],
[ 1. , 0. , 0. , 0.82216251]])
>>> Xd1
array([[-0.63687136],
[ 0.36049576],
[-1.61955115],
[ 0.08980563],
[-0.46080748],
[-0.60043404]])
>>> Xd2
array([[-0.63687136],
[ 0.36049576],
[-1.61955115],
[ 0.08980563],
[-0.46080748],
[-0.60043404]])For the case of velocities calculated with dual quaternions, it is possible to use the Dual Inertial Velcocity Jacobian Matrix, because this maps the effect of each joint directly to the end-effector in dual space, so linear and angular velocities can be calculated:
This can be calculated with the library as follows:
"""
Total Inertial Velocity (using Dual Quaternions)
"""
# Differential Kinematics library
from lib.kinematics.DifferentialDQ import *
# NumPy
import numpy as np
# Dual Inertial Velocity Jacobian Matrix
Jvdq = jacobianVelocityDQ(uRobot, symbolic = False)
# Total Inertial Velocity (calculated by Dual Inertial Velocity Jacobian Matrix and joints velocities)
dqXd1 = Jvdq.dot(uRobot.jointsVelocities)
# Total Dual Inertial Velocity (calculated by library's module)
dqXd2 = geometricStateSpace(uRobot, symbolic = False)So the outputs will be
# NumPy Array
>>> Jvdq
array([[ 0. , 0. , 0. , 0. ],
[ 0. , -0.54368308, -0.54368308, -0.47776856],
[ 0. , -0.8392906 , -0.8392906 , 0.30949314],
[ 1. , 0. , 0. , 0.82216251],
[ 0. , 0. , 0. , 0. ],
[ 0.18311838, -0.38379045, -0.62048222, 0. ],
[ 0.2826822 , 0.24861517, 0.40194146, 0. ],
[ 0. , 0.33681088, -0.51187571, 0. ]])
>>> dqXd1
array([[ 0. ],
[ 0.08980563],
[-0.46080748],
[-0.60043404],
[ 0. ],
[-0.63687136],
[ 0.36049576],
[-1.61955115]])
>>> dqXd2
array([[ 0. ],
[ 0.08980563],
[-0.46080748],
[-0.60043404],
[ 0. ],
[-0.63687136],
[ 0.36049576],
[-1.61955115]])Please notice that angular velocities are not the same as the angular rate of change on Total Inertial Rate of Change results. You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
The simplest and fastest way to calculate the angular and linear velocity of the reference frames attached to each joint is by means of a recursive algorithm, whose premise is to analyze the velocities from the base of the robot to the end-effector with the following equations:
,
,
where are the inertial angular velocities of the i - th frame and the subsequent one; on the other hand,
is the axis of actuation of the i - th joint
. Also,
are the inertial linear velocities of the i - th frame and the subsequent one, meanwhile
represents the relative position between two reference frames (they can be calculated with the Forward Kinematics algorithm). Based on the Screw Theory, both angular and linear velocities can be calculated as follows:
with , representing an identity matrix and a zeros one respectively. With the aforementioned terms, velocity propagation can be calculated with the library as follows:
"""
Inertial Velocity Propagation
"""
# Differential Kinematics library
from lib.kinematics.DifferentialHTM import *
# NumPy
import numpy as np
# Inertial velocity propagation to each reference frame
V = velocityPropagation(uRobot, v0 = np.zeros((3, 1)), w0 = np.zeros((3, 1)), qd = qd, symbolic = False)So the outputs will be
# NumPy Array
>>> V[0]
array([[0.],
[0.],
[0.],
[0.],
[0.],
[0.]])
>>> V[1]
array([[ 0. ],
[ 0. ],
[ 0. ],
[ 0. ],
[ 0. ],
[-0.129733]])
>>> V[2]
array([[-0.463299 ],
[ 0.16893454],
[-1.44657525],
[ 0.92670074],
[ 1.4305599 ],
[-0.129733 ]])
>>> V[3]
array([[-0.463299 ],
[ 0.16893454],
[-1.44657525],
[-0.18372442],
[-0.28361776],
[-0.129733 ]])
>>> V[4]
array([[-0.63687136],
[ 0.36049576],
[-1.61955115],
[ 0.08980563],
[-0.46080748],
[-0.60043404]])Please notice that initial angular and linear velocities (w0 and v0) were set to zero because the base of the robot doesn't move; on the other hand, Python will send all the velocities in a list. You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
Dual Quaternions can be used to represent the relative pose of a reference frame, therefore they can be used to calculate its velocity. In this case, a dual quaternion contains the angular and linear velocity, from the base to the end-effector, in a single result with the following equation:
where are the angular and linear velocities of the i + 1 frame, meanwhile
represent an identity matrix and a zeros one respectively. Moreover,
is the dual quaternion that represent the pose of the i-th frame and
is the screw vector that represent the axis of actuation of the joint
(if any).
On the other hand, are the relative positions, expressed as quaternions, of the frames with respect to the inertial one and between i and i + 1. These can be calculated with the Dual Quaternions to Euclidian Space functionality.
With the aforementioned terms, velocity propagation using dual quaternions can be calculated with the library as follows:
"""
Inertial Velocity Propagation using Dual Quaternions
"""
# Differential Kinematics library
from lib.kinematics.DifferentialDQ import *
# NumPy
import numpy as np
# Inertial velocity propagation to each reference frame using dual quaternions
Wdq = dqVelocityPropagation(uRobot, w0 = np.zeros((8, 1)), qd = qd, symbolic = False)So the outputs will be
# NumPy Array
>>> Wdq[0]
array([[0.],
[0.],
[0.],
[0.],
[0.],
[0.],
[0.],
[0.]])
>>> Wdq[1]
array([[ 0. ],
[ 0. ],
[ 0. ],
[-0.129733],
[ 0. ],
[ 0. ],
[ 0. ],
[ 0. ]])
>>> Wdq[2]
array([[ 0. ],
[ 0.92670074],
[ 1.4305599 ],
[-0.129733 ],
[ 0. ],
[-0.463299 ],
[ 0.16893454],
[-1.44657525]])
>>> Wdq[3]
array([[ 0. ],
[-0.18372442],
[-0.28361776],
[-0.129733 ],
[ 0. ],
[-0.463299 ],
[ 0.16893454],
[-1.44657525]])
>>> Wdq[4]
array([[ 0. ],
[ 0.08980563],
[-0.46080748],
[-0.60043404],
[ 0. ],
[-0.63687136],
[ 0.36049576],
[-1.61955115]])Please notice that initial velocity w0 was set to zero because the base of the robot doesn't move. To check if the results are correct, you can run Inertial Angular Velocity Propagation and Inertial Linear Velocity Propagation algorithms. You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
End-effector acceleration can be calculated with Geometric Jacobian Matrix and its derivative, because this maps the effect of each joint directly to the end-effector, so linear and angular accelerations can be calculated:
This can be calculated with the library as follows:
"""
Total Inertial Acceleration
"""
# Differential Kinematics library
from lib.kinematics.HTM import *
from lib.kinematics.DifferentialHTM import *
# NumPy
import numpy as np
# Geometric Jacobian Matrix (OPTIONAL)
Jg = geometricJacobian(uRobot, symbolic = False)
# Time Derivative of Geometric Jacobian Matrix (OPTIONAL)
dJg = geometricJacobianDerivative(uRobot, symbolic = False)
# Total Inertial Acceleration
geometricXdd1 = dJg.dot(uRobot.jointsVelocities) + Jg.dot(uRobot.jointsAccelerations)
# Total Inertial Acceleration
geometricXdd2 = geometricDerivativeStateSpace(uRobot, symbolic = False)So the outputs will be
#NumPy Array
>>> Jg
array([[ 0.18311838, -0.38379045, -0.62048222, 0. ],
[ 0.2826822 , 0.24861517, 0.40194146, 0. ],
[ 0. , 0.33681088, -0.51187571, 0. ],
[ 0. , -0.54368308, -0.54368308, -0.47776856],
[ 0. , -0.8392906 , -0.8392906 , 0.30949314],
[ 1. , 0. , 0. , 0.82216251]])
>>> dJg
array([[-0.16554583, 1.70367461, 0.52936056, 0.2110293 ],
[ 0.10723874, -0.34886697, 0.49902499, 0.22534713],
[-0.1668235 , -0.73051559, -0.24982624, 0.03780247],
[ 0.46080748, 0.39505496, 0.39505496, 0. ],
[ 0.08980563, -0.25591219, -0.25591219, 0. ],
[ 0. , 0.32590626, 0.32590626, 0. ]])
>>> geometricXdd1
array([[-3.68057734],
[ 2.62284587],
[-0.94773301],
[-1.41937091],
[-2.36093456],
[ 0.17500924]])
>>> geometricXdd2
array([[-3.68057734],
[ 2.62284587],
[-0.94773301],
[-1.41937091],
[-2.36093456],
[ 0.17500924]])For the case of accelerations calculated with dual quaternions, it is possible to use the Time Derivative of Dual Inertial Velcocity Jacobian Matrix, because this maps the effect of each joint directly to the end-effector in dual space, so linear and angular accelerations can be calculated:
This can be calculated with the library as follows:
"""
Total Inertial Acceleration (using Dual Quaternions)
"""
# Differential Kinematics library
from lib.kinematics.DQ import *
from lib.kinematics.DifferentialDQ import *
# NumPy
import numpy as np
# Dual Inertial Velocity Jacobian Matrix (OPTIONAL)
Jvdq = jacobianVelocityDQ(uRobot, symbolic = False)
# Time Derivative of Dual Inertial Velocity Jacobian Matrix (OPTIONAL)
dJvdq = jacobianVelocityDerivativeDQCOM(uRobot, symbolic = False)
# Total Inertial Acceleration
dqXdd1 = dJvdq.dot(uRobot.jointsVelocities) + JvdqCOM.dot(uRobot.jointsAccelerations)
# Total Inertial Acceleration
dqXdd = dqDerivativeStateSpace(uRobot, symbolic = False)So the outputs will be
# NumPy Array
>>> Jvdq
array([[ 0. , 0. , 0. , 0. ],
[ 0. , -0.54368308, -0.54368308, -0.47776856],
[ 0. , -0.8392906 , -0.8392906 , 0.30949314],
[ 1. , 0. , 0. , 0.82216251],
[ 0. , 0. , 0. , 0. ],
[ 0.18311838, -0.38379045, -0.62048222, 0. ],
[ 0.2826822 , 0.24861517, 0.40194146, 0. ],
[ 0. , 0.33681088, -0.51187571, 0. ]])
>>> dJvdq
array([[ 0. , 0. , 0. , 0. ],
[ 0. , -0.10888369, -0.10888369, -0.19302842],
[ 0. , 0.07053364, 0.07053364, 0.21303368],
[ 0. , 0. , 0. , -0.1923651 ],
[ 0. , 0. , 0. , 0. ],
[-0.36049576, 1.39152764, 0.19732212, 0. ],
[-0.63687136, -0.83073227, -0.01354705, 0. ],
[ 0. , -0.73051559, -0.24982624, 0. ]])
>>> dqXdd1
array([[ 0. ],
[-1.41937091],
[-2.36093456],
[ 0.17500924],
[ 0. ],
[-3.68057734],
[ 2.62284587],
[-0.94773301]])
>>> dqXdd2
array([[ 0. ],
[-1.41937091],
[-2.36093456],
[ 0.17500924],
[ 0. ],
[-3.68057734],
[ 2.62284587],
[-0.94773301]])You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
Linear and angular accelerations can also be calculated recursively. In this case, acceleration propagation can be analyzed from the base of the robot to the end-effector with the following equations:
,
,
where are the inertial angular and linear accelerations of the i - th frame and the subsequent one; on the other hand,
are the axis of actuation of the i - th joint and its relative position. Based on the Screw Theory, both angular and linear accelerations can be calculated as follows:
with , representing an identity matrix and a zeros one respectively. With the aforementioned terms, acceleration propagation can be calculated with the library as follows:
"""
Inertial Acceleration Propagation
"""
# Differential Kinematics library
from lib.kinematics.DifferentialHTM import *
# NumPy
import numpy as np
# Inertial acceleration propagation to each reference frame
dV = accelerationPropagation(uRobot, dv0 = np.zeros((3, 1)), dw0 = np.zeros((3, 1)), V = V, qd = qd, qdd = qdd, symbolic = False)So the outputs will be
# NumPy Array
>>> dV[0]
array([[0.],
[0.],
[0.],
[0.],
[0.],
[0.]])
>>> dV[1]
array([[0. ],
[0. ],
[0. ],
[0. ],
[0. ],
[0.03182287]])
>>> dV[2]
array([[-2.08117708],
[ 1.52894714],
[ 0.52434241],
[ 0.37456409],
[ 0.1714968 ],
[ 0.03182287]])
>>> dV[3]
array([[-2.08117708],
[ 1.52894714],
[ 0.52434241],
[-1.51067447],
[-2.2514123 ],
[ 0.03182287]])
>>> dV[4]
array([[-3.68057734],
[ 2.62284587],
[-0.94773301],
[-1.41937091],
[-2.36093456],
[ 0.17500924]])Please notice that initial angular and linear accelerations (dw0 and dv0) were set to zero because the base of the robot doesn't move; also the list with the velocities V has to be sent as a parameter of this function, so Inertial Velocity Propagation has to be computed before calling this function. On the other hand, Python will send all the accelerations inside a list. You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
Dual Quaternions can be used to represent the relative pose of a reference frame, therefore they can be used to calculate its acceleration. In this case, a dual quaternion contains the angular and linear accelerations, from the base to the end-effector, in a single result with the following equation:
where are the angular and linear accelerations of the i + 1 frame, meanwhile
represent an identity matrix, a zeros one and a zeros vector respectively. Moreover,
is the dual quaternion that represent the pose of the i-th frame and
are the screw vector and its time derivative, that represent the axis of actuation of the joint
(if any) and its rate of change.
On the other hand, are the relative positions, expressed as quaternions, of the frames with respect to the inertial one and between i and i + 1. These can be calculated with the Dual Quaternions to Euclidian Space functionality. Furthermore, it's mandatory to calculate the Inertial Velocity Propagation Using Dual Quaternions to get the angular and linear velocities of the i-th frame
.
With the aforementioned terms, acceleration propagation using dual quaternions can be calculated with the library as follows:
"""
Inertial Acceleration Propagation using Dual Quaternions
"""
# Differential Kinematics library
from lib.kinematics.DifferentialDQ import *
# NumPy
import numpy as np
# Inertial acceleration propagation using Dual Quaternions
dWdq = dqAccelerationPropagation(uRobot, dw0 = np.zeros((8, 1)), Wdq = Wdq, qd = qd, qdd = qdd, symbolic = False)So the outputs will be
# NumPy Array
>>> dWdq[0]
array([[0.],
[0.],
[0.],
[0.],
[0.],
[0.],
[0.],
[0.]])
>>> dWdq[1]
array([[0. ],
[0. ],
[0. ],
[0.03182287],
[0. ],
[0. ],
[0. ],
[0. ]])
>>> dWdq[2]
array([[ 0. ],
[ 0.37456409],
[ 0.1714968 ],
[ 0.03182287],
[ 0. ],
[-2.08117708],
[ 1.52894714],
[ 0.52434241]])
>>> dWdq[3]
array([[ 0. ],
[-1.51067447],
[-2.2514123 ],
[ 0.03182287],
[ 0. ],
[-2.08117708],
[ 1.52894714],
[ 0.52434241]])
>>> dWdq[4]
array([[ 0. ],
[-1.41937091],
[-2.36093456],
[ 0.17500924],
[ 0. ],
[-3.68057734],
[ 2.62284587],
[-0.94773301]])Please notice that initial acceleration dw0 was set to zero because the base of the robot doesn't move. To check if the results are correct, you can run Inertial Acceleration Propagation algorithm. You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
For dynamic modelling, it will be mandatory to know the velocity of each center of mass. As stated in previous sections, inertial velocities can be calculated with Geometric Jacobian Matrix. In this case, it maps the effect of each joint directly to the each center of mass, so linear and angular velocities can be calculated:
This can be calculated with the library as follows:
"""
Inertial Velocity to a Center of Mass
"""
# Differential Kinematics library
from lib.kinematics.DifferentialHTM import *
# NumPy
import numpy as np
# Geometric Jacobian Matrix for a specific center of mass
JgCOM = geometricJacobianCOM(uRobot, COM = 2, symbolic = False)
# Inertial Velocity (calculated by geometric jacobian to center of mass and joints velocities)
geometricXdCOM1 = JgCOM.dot(uRobot.jointsVelocities)
# Inertial Velocity (calculated by library's module)
geometricXdCOM2 = geometricCOMStateSpace(uRobot, COM = 2, symbolic = False)So the outputs will be
# NumPy Array
>>> JgCOM
array([[ 0.23070827, 0.11834589, 0. , 0. ],
[ 0.35614734, -0.07666314, 0. , 0. ],
[ 0. , 0.42434329, 0. , 0. ],
[ 0. , -0.54368308, 0. , 0. ],
[ 0. , -0.8392906 , 0. , 0. ],
[ 1. , 0. , 0. , 0. ]])
>>> geometricXdCOM1
array([[-0.2316495 ],
[ 0.08446727],
[-0.72328762],
[ 0.92670074],
[ 1.4305599 ],
[-0.129733 ]])
>>> geometricXdCOM2
array([[-0.2316495 ],
[ 0.08446727],
[-0.72328762],
[ 0.92670074],
[ 1.4305599 ],
[-0.129733 ]])For the case of velocities calculated with dual quaternions, it is possible to use the Dual Inertial Velcocity Jacobian Matrix, because this maps the effect of each joint directly to the end-effector in dual space, so linear and angular velocities can be calculated:
This can be calculated with the library as follows:
"""
Total Inertial Velocity (using Dual Quaternions)
"""
# Differential Kinematics library
from lib.kinematics.DQ import *
from lib.kinematics.DifferentialDQ import *
# NumPy
import numpy as np
# Dual Inertial Velocity Jacobian Matrix
JvdqCOM = jacobianVelocityDQCOM(uRobot, COM = 2, symbolic = False)
# Total Inertial Velocity (calculated by Dual Inertial Velocity Jacobian Matrix and joints velocities)
dqXd1COM = JvdqCOM.dot(uRobot.jointsVelocities)
# Total Dual Inertial Velocity (calculated by library's module)
dqXd2COM = dqStateSpaceCOM(uRobot, COM = 2, symbolic = Fals)So the outputs will be
# NumPy Array
>>> JvdqCOM
array([[ 0. , 0. , 0. , 0. ],
[ 0. , -0.54368308, 0. , 0. ],
[ 0. , -0.8392906 , 0. , 0. ],
[ 1. , 0. , 0. , 0. ],
[ 0. , 0. , 0. , 0. ],
[ 0.23070827, 0.11834589, 0. , 0. ],
[ 0.35614734, -0.07666314, 0. , 0. ],
[ 0. , 0.42434329, 0. , 0. ]])
>>> dqXd1COM
array([[ 0. ],
[ 0.92670074],
[ 1.4305599 ],
[-0.129733 ],
[ 0. ],
[-0.2316495 ],
[ 0.08446727],
[-0.72328762]])
>>> dqXd2COM
array([[ 0. ],
[ 0.92670074],
[ 1.4305599 ],
[-0.129733 ],
[ 0. ],
[-0.2316495 ],
[ 0.08446727],
[-0.72328762]])Please notice that jacobian matrix is zero in columns two to four because these joints () don't affect the center of mass number two because they are attached after it. You can also calculate its symbolic expression by setting
symbolic parameter to True, but this may be slow
The velocity propagation can be used to analize centers of mass. In this case, angular velocity from the base up to each center of mass is defined with the following equation:
,
,
where are the inertial angular velocities of the i - th frame and the one for the j - th rigid body; on the other hand,
is the axis of actuation of the i - th joint
. Also,
are the inertial linear velocities of the i - th frame and the j - th reference frame, meanwhile
represents the relative position between two reference frames (their positions can be obtained from the Forward Kinematics algorithm). Based on the Screw Theory, both angular and linear velocities to the centers of mass can be calculated as follows:
with , representing an identity matrix and a zeros one respectively. With the aforementioned terms, acceleration propagation can be calculated with the library as follows:
"""
Inertial Velocity Propagation to Centers of Mass
"""
# Differential Kinematics library
from lib.kinematics.DifferentialHTM import *
# NumPy
import numpy as np
# Inertial velocity propagation to each center of mass
Vcom = velocityPropagationCOM(uRobot, vCOM0 = np.zeros((3, 1)), wCOM0 = np.zeros((3, 1)), V = V, qd = qd, symbolic = False)So the outputs will be
# NumPy Array
>>> Vcom[0]
array([[0.],
[0.],
[0.],
[0.],
[0.],
[0.]])
>>> Vcom[1]
array([[ 0. ],
[ 0. ],
[ 0. ],
[ 0. ],
[ 0. ],
[-0.129733]])
>>> Vcom[2]
array([[-0.2316495 ],
[ 0.08446727],
[-0.72328762],
[ 0.92670074],
[ 1.4305599 ],
[-0.129733 ]])
>>> Vcom[3]
array([[-0.55008518],
[ 0.26471515],
[-1.5330632 ],
[ 0.08980563],
[-0.46080748],
[-0.60043404]])Please notice that initial linear and angular velocitis (vCOM0 and wCOM0) were set to zero because the base of the robot doesn't move; also it's mandatory to calculate angular velocities to each reference frame V before using this function, this is because we have to send the results as parameters. On the other hand, Python will send all the angular velocities in a list. You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
Dual Quaternions can be used to represent the relative pose of a center of mass, therefore they can be used to calculate its velocity. In this case, a dual quaternion contains the angular and linear velocity, from the base to the j-th center of mass, in a single result with the following equation:
where are the angular and linear velocities of the j-th center of mass, meanwhile
represent an identity matrix and a zeros one respectively. Moreover,
is the dual quaternion that represent the pose of the i-th frame and
is the screw vector that represent the axis of actuation of the joint
(if any).
On the other hand, are the relative positions, expressed as quaternions, of the j-th center of mass with respect to the inertial frame and the i-th one. These can be calculated with the Dual Quaternions to Euclidian Space functionality.
With the aforementioned terms, velocity propagation using dual quaternions can be calculated with the library as follows:
"""
Inertial Velocity Propagation to Centers of Mass using Dual Quaternions
"""
# Differential Kinematics library
from lib.kinematics.DifferentialDQ import *
# NumPy
import numpy as np
# Inertial velocity propagation to each center of mass using Dual Quaternions
WdqCOM = dqVelocityPropagationCOM(uRobot, WdqCOM0 = np.zeros((8, 1)), Wdq = Wdq, qd = qd, symbolic = False)So the outputs will be
# NumPy Array
>>> WdqCOM[0]
array([[0.],
[0.],
[0.],
[0.],
[0.],
[0.],
[0.],
[0.]])
>>> WdqCOM[1]
array([[ 0. ],
[ 0. ],
[ 0. ],
[-0.129733],
[ 0. ],
[ 0. ],
[ 0. ],
[ 0. ]])
>>> WdqCOM[2]
array([[ 0. ],
[ 0.92670074],
[ 1.4305599 ],
[-0.129733 ],
[ 0. ],
[-0.2316495 ],
[ 0.08446727],
[-0.72328762]])
>>> WdqCOM[3]
array([[ 0. ],
[ 0.08980563],
[-0.46080748],
[-0.60043404],
[ 0. ],
[-0.55008518],
[ 0.26471515],
[-1.5330632 ]])Please notice that initial velocity WdqCOM0 was set to zero because the base of the robot doesn't move. To check if the results are correct, you can run Inertial Velocity Propagation to Centers of Mass algorithm. You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
For dynamic modelling, it will be mandatory to know the velocity of each center of mass. As stated in previous sections, inertial accelerations can be calculated with Geometric Jacobian Matrix and its time derivative. In this case, it maps the effect of each joint directly to the each center of mass, so linear and angular accelerations can be calculated:
This can be calculated with the library as follows:
"""
Inertial Acceleration to a Center of Mass
"""
# Differential Kinematics library
from lib.kinematics.DifferentialHTM import *
# NumPy
import numpy as np
# Geometric Jacobian Matrix (OPTIONAL)
JgCOM = geometricJacobianCOM(uRobot, COM = 2, symbolic = False)
# Time Derivative of Geometric Jacobian Matrix (OPTIONAL)
dJgCOM = geometricJacobianDerivative(uRobot, COM = 2, symbolic = False)
# Total Inertial Acceleration
geometricXdd1COM = dJgCOM.dot(uRobot.jointsVelocities) + JgCOM.dot(uRobot.jointsAccelerations)
# Total Inertial Acceleration
geometricXdd2COM = geometricDerivativeStateSpaceCOM(uRobot, COM = 2, symbolic = False)So the outputs will be
# NumPy Array
>>> JgCOM
array([[ 0.23070827, 0.11834589, 0. , 0. ],
[ 0.35614734, -0.07666314, 0. , 0. ],
[ 0. , 0.42434329, 0. , 0. ],
[ 0. , -0.54368308, 0. , 0. ],
[ 0. , -0.8392906 , 0. , 0. ],
[ 1. , 0. , 0. , 0. ]])
>>> dJgCOM
array([[ 0.04620406, 0.58715702, 0. , 0. ],
[-0.02993048, -0.42394598, 0. , 0. ],
[ 0. , -0.24034467, 0. , 0. ],
[-1.4305599 , 0. , 0. , 0. ],
[ 0.92670074, 0. , 0. , 0. ],
[ 0. , 0. , 0. , 0. ]])
>>> geometricXdd1COM
array([[-1.04058854],
[ 0.76447357],
[ 0.2621712 ],
[ 0.37456409],
[ 0.1714968 ],
[ 0.03182287]])
>>> geometricXdd2COM
array([[-1.04058854],
[ 0.76447357],
[ 0.2621712 ],
[ 0.37456409],
[ 0.1714968 ],
[ 0.03182287]])For the case of accelerations calculated with dual quaternions, it is possible to use the Time Derivative of Dual Inertial Velcocity Jacobian Matrix, because this maps the effect of each joint directly to the end-effector in dual space, so linear and angular accelerations can be calculated:
This can be calculated with the library as follows:
"""
Total Inertial Acceleration (using Dual Quaternions)
"""
# Differential Kinematics library
from lib.kinematics.DQ import *
from lib.kinematics.DifferentialDQ import *
# NumPy
import numpy as np
# Geometric Jacobian Matrix to any Center of Mass (OPTIONAL)
JvdqCOM = jacobianVelocityDQCOM(uRobot, COM = 2, symbolic = False)
# Time Derivative of Dual Inertial Velocity Jacobian Matrix to any Center of Mass (OPTIONAL)
dJvdqCOM = jacobianVelocityDerivativeDQCOM(uRobot, COM = 2, symbolic = False)
# Total Inertial Acceleration
dqXdd1COM = dJvdqCOM.dot(uRobot.jointsVelocities) + JvdqCOM.dot(uRobot.jointsAccelerations)
# Inertial acceleration of each Center of Mass (using dual inertial acceleration jacobian matrix) with Dual Quaternions
dqXddCOM = dqDerivativeStateSpaceCOM(uRobot, COM = 2, symbolic = False)So the outputs will be
# NumPy Array
>>> JvdqCOM
array([[ 0. , 0. , 0. , 0. ],
[ 0. , -0.54368308, 0. , 0. ],
[ 0. , -0.8392906 , 0. , 0. ],
[ 1. , 0. , 0. , 0. ],
[ 0. , 0. , 0. , 0. ],
[ 0.23070827, 0.11834589, 0. , 0. ],
[ 0.35614734, -0.07666314, 0. , 0. ],
[ 0. , 0.42434329, 0. , 0. ]])
>>> dJvdqCOM
array([[ 0. , 0. , 0. , 0. ],
[ 0. , -0.10888369, 0. , 0. ],
[ 0. , 0.07053364, 0. , 0. ],
[ 0. , 0. , 0. , 0. ],
[ 0. , 0. , 0. , 0. ],
[-0.08446727, 0.59710276, 0. , 0. ],
[-0.2316495 , -0.40859261, 0. , 0. ],
[ 0. , -0.24034467, 0. , 0. ]])
>>> dqXdd1COM
array([[ 0. ],
[ 0.37456409],
[ 0.1714968 ],
[ 0.03182287],
[ 0. ],
[-1.04058854],
[ 0.76447357],
[ 0.2621712 ]])
>>> dqXdd2COM
array([[ 0. ],
[ 0.37456409],
[ 0.1714968 ],
[ 0.03182287],
[ 0. ],
[-1.04058854],
[ 0.76447357],
[ 0.2621712 ]])Please notice that jacobian matrix is zero in columns two to four because these joints () don't affect the center of mass number two because they are attached after it. You can also calculate its symbolic expression by setting
symbolic parameter to True, but this may be slow
Acelerations can also be calculated recursively. In this case, angular acceleration of each center of mass can be analyzed from the base of the robot to the end-effector with the following equation:
,
,
where are the inertial angular accelerations of the i - th frame and the j - th rigid body; on the other hand,
is the axis of actuation of the i - th joint
. Also,
are the inertial linear accelerations of the i - th frame and the j - th rigid body; on the other hand,
are the angular velocities and accelerations calculated as shown in previous sections, meanwhile
represents the relative position between two reference frames (their positions can be obtained from the Forward Kinematics algorithm). Based on the Screw Theory, both angular and linear velocities to the centers of mass can be calculated as follows:
with , representing an identity matrix and a zeros one respectively. With the aforementioned terms, acceleration propagation can be calculated with the library as follows:
"""
Inertial Acceleration Propagation to Centers of Mass
"""
# Differential Kinematics library
from lib.kinematics.DifferentialHTM import *
# NumPy
import numpy as np
# Inertial acceleration propagation to each reference frame
dVcom = accelerationPropagationCOM(uRobot, dvCOM0 = np.zeros((3, 1)), dwCOM0 = np.zeros((3, 1)), Vcom = Vcom, dV = dV, qd = qd, qdd = qdd)So the outputs will be
# NumPy Array
>>> dVcom[0]
array([[0.],
[0.],
[0.],
[0.],
[0.],
[0.]])
>>> dVcom[1]
array([[0. ],
[0. ],
[0. ],
[0. ],
[0. ],
[0.03182287]])
>>> dVcom[2]
array([[-1.04058854],
[ 0.76447357],
[ 0.2621712 ],
[ 0.37456409],
[ 0.1714968 ],
[ 0.03182287]])
>>> dVcom[3]
array([[-2.88087721],
[ 2.07589651],
[-0.2116953 ],
[-1.41937091],
[-2.36093456],
[ 0.17500924]])Please notice that initial angular and linear accelerations (dWCOM0 and dvCOM0) were set to zero because the base of the robot doesn't move; also it's mandatory to calculate the velocities to each center of mass Vcom and the acceleration of each reference frame dV before using this function, this is because we have to send the results as parameters. On the other hand, Python will send all the accelartions in a list. You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
Dual Quaternions can be used to represent the relative pose of a center of mass, therefore they can be used to calculate its acceleration. In this case, a dual quaternion contains the angular and linear accelerations, from the base to the center of mass, in a single result with the following equation:
where are the angular and linear accelerations of the j-th center of mass, meanwhile
represent an identity matrix, a zeros one and a zeros vector respectively. Moreover,
is the dual quaternion that represent the pose of the i-th frame and
are the screw vector and its time derivative, that represent the axis of actuation of the joint
(if any) and its rate of change.
On the other hand, are the relative positions, expressed as quaternions, of the i-th reference frame and the j-th center of mass, with respect to the inertial frame and the i-th one. These can be calculated with the Dual Quaternions to Euclidian Space functionality. Furthermore, it's mandatory to calculate the Inertial Velocity Propagation Using Dual Quaternions to get the angular and linear velocities of the i-th frame
.
With the aforementioned terms, acceleration propagation to centers of mass using dual quaternions can be calculated with the library as follows:
"""
Inertial Acceleration Propagation to Centers of Mass using Dual Quaternions
"""
# Differential Kinematics library
from lib.kinematics.DifferentialDQ import *
# NumPy
import numpy as np
# Inertial acceleration propagation to each center of mass using Dual Quaternions
dWdqCOM = dqAccelerationPropagationCOM(uRobot, dWdqCOM0 = np.zeros((8, 1)), Wdq = Wdq, WdqCOM = WdqCOM, dWdq = dWdq, qd = qd, qdd = qdd, symbolic = False)So the outputs will be
# NumPy Array
>>> dWdqCOM[0]
array([[0.],
[0.],
[0.],
[0.],
[0.],
[0.],
[0.],
[0.]])
>>> dWdqCOM[1]
array([[0. ],
[0. ],
[0. ],
[0.03182287],
[0. ],
[0. ],
[0. ],
[0. ]])
>>> dWdqCOM[2]
array([[ 0. ],
[ 0.37456409],
[ 0.1714968 ],
[ 0.03182287],
[ 0. ],
[-1.04058854],
[ 0.76447357],
[ 0.2621712 ]])
>>> dWdqCOM[3]
array([[ 0. ],
[-1.41937091],
[-2.36093456],
[ 0.17500924],
[ 0. ],
[-2.88087721],
[ 2.07589651],
[-0.2116953 ]])Please notice that initial acceleration dWdqCOM0 was set to zero because the base of the robot doesn't move. To check if the results are correct, you can run Inertial Angular Acceleration Propagation and Inertial Linear Acceleration Propagation to Centers of Mass algorithms. You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
Robot dynamics studies the forces acting on a robotic mechanism and the accelerations that these forces produce. The robotic mechanism is generally considered as a rigid system
It describes the behavior of a dynamical system in terms of the work and energy stored in it, rather than the forces and moments of the individual members involved. It can be described as follows:
Where represents the torques applied to each joint or generalized coordinate. Also,
is the Lagrangian, a scalar representation of the relationship between kinetic
and potential energy
, this is
In robotics, this is analyzed with respect to each Center of Mass because the way forces and torques propagates through each rigid body. To calculate it, previous equations can be rewriten as
, where
To calculate each term, Forward Kinematics and Differential Kinematics will be used, so dynamic modelling won't be too complicated if previous topics were thoroughly studied 😃
This is a form of energy that a rigid body has by its motion. If a rigid body is affected by an external force, it speeds up and thereby gains kinetic energy, so the one of the i - th element, with respect to its center of mass, is defined as
, with
where that can be obtained as shown here or here:
, or
, where
and
therefore, first equation can be rewriten as follows:
.
Then, total kinetic energy is the sum of all energies, this is:
,
where is known as the inertia matrix, who is a square symmetric matrix that will be useful for further calculations. Kinetic energy can be calculated with the library as follows:
"""
Robot's Total Kinetic Energy with respect to each center of mass
"""
# Dynamics library
from lib.dynamics.DynamicsHTM import *
from lib.dynamics.DynamicsDQ import *
# Kinetic Energy of the robot in the Centers of Mass: 0.5 * q'(t)^T * D * q'(t)
K = kineticEnergyCOM(uRobot, symbolic = False)
Kdq = dqKineticEnergyCOM(uRobot, symbolic = False)So the output will be
# NumPy Arrays
>>> K
array([[0.79503744]])
>>> Kdq
array([[0.79503744]])You can also calculate the full symbolic expression by setting symbolic parameters to True, but this may be slow
Basically, this is a representation of the sum of linear and angular momentums in the robot, this leads to a symmetric matrix. As shown in previous sections, this matrix can be calculated as
The algorithms in this library requires the calculation of Forward Kinematics to Center of Mass and Jacobian Matrix to Center of Mass that was discussed in this and this sections. Also, given each inertia tensor on the attributes section, this algorithm will transform it into
automatically 😉 (or its equivalent in quaternion space)
"""
Robot's Inertia Matrix (not the same as Inertia Tensor)
"""
# Dynamics library
from lib.dynamics.DynamicsHTM import *
from lib.dynamics.DynamicsDQ import *
# Inertia Matrix for Kinetic Energy equation: D(q)
D = inertiaMatrixCOM(uRobot, symbolic = False)
Ddq = dqInertiaMatrixCOM(uRobot, symbolic = False)So the output will be
# NumPy Array
>>> D
array([[ 1.73561594, 1.23248094, 0.61313663, -0.77738321],
[ 1.23248094, 2.54573558, 0.91515058, -0.58521715],
[ 0.61313663, 0.91515058, 0.86025536, -0.58521715],
[-0.77738321, -0.58521715, -0.58521715, 0.73863435]])
>>> Ddq
array([[ 1.73561594, 1.23248094, 0.61313663, -0.77738321],
[ 1.23248094, 2.54573558, 0.91515058, -0.58521715],
[ 0.61313663, 0.91515058, 0.86025536, -0.58521715],
[-0.77738321, -0.58521715, -0.58521715, 0.73863435]])You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
This is only a linear transformation of the matrix shown in the previous section. To calculate it, is necessary to calculate the Geometric Jacobian Matrix of the robot and then multiply it as follows
The algorithm in this library calculates the Inertia Matrix in Joints Space automatically when you call it 😉
"""
Robot's Inertia Matrix in Cartesian Space (not the same as Inertia Tensor)
"""
# Dynamics library
from lib.dynamics.DynamicsHTM import *
# Inertia Matrix in Cartesian Space: M(q)
D = inertiaMatrixCartesian(uRobot, symbolic = False)So the output will be
# NumPy Array
>>> M
array([[ 1.67620328e+02, 1.68648740e+02, -2.36731217e+02, -4.34024667e+00, 2.63005237e+00, -1.07386275e+00],
[ 1.68648740e+02, 1.70134621e+02, -2.38434092e+02, -4.06495761e+00, 2.82298081e+00, -6.05471808e-01],
[-2.36731217e+02, -2.38434092e+02, 3.34940579e+02, 6.31943441e+00, -4.38037279e+00, 1.41511316e+00],
[-4.34024667e+00, -4.06495761e+00, 6.31943441e+00, 5.18404749e-01, -4.58143258e-01, 5.76949173e-01],
[ 2.63005237e+00, 2.82298081e+00, -4.38037279e+00, -4.58143258e-01, 7.31741764e-01, 7.91485515e-02],
[-1.07386275e+00, -6.05471808e-01, 1.41511316e+00, 5.76949173e-01, 7.91485515e-02, 4.02976991e-01]])You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
This is the energy stored in an object due to its position relative to the inertial one. This is described as
,
where is the gravity acceleration with respect to inertial frame, usually defined as
or
if this energy is calculated using dual quaternions. Gravitational acceleration is constant and have to be defined depending on the orientation of your inertial frame. Moreover
is the position of the i - th center of mass that can be obtained as shown here or here. The total potential energy can be calculated as follows:
You can do this task with the library as shown below:
"""
Robot's Total Potential Energy with respect to each center of mass
"""
# Dynamics library
from lib.dynamics.DynamicsHTM import *
from lib.dynamics.DynamicsDQ import *
# Potential Energy of the robot in the Centers of Mass: m * g^T * r (OPTIONAL)
P = potentialEnergyCOM(uRobot, symbolic = False)
PDQ = dqPotentialEnergyCOM(uRobot, symbolic = False)So the output will be
# NumPy Array
>>> P
array([-1.60453458])
>>> Pdq
array([-1.60453458])You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
This is as simple as calculating the difference between total kinetic energy and total potential one
,
Last equation will be useful to calculate the dynamical representation of a robot 🤖
As part of the Euler - Lagrange formulation, it is mandatory to calculate the rate of change of the lagrangian with respect to the joints positions, so this leads to
Using the equation shown in this section, is possible to expand previous result as
where is the vector with the gravitational effects, that is the derivative of potential energy with respect to each joint
, who is defined as
or
if you're using dual quaternions. This is a rectangular matrix with the derivatives of the centers of mass' positions. The rest of the terms will be discussed in the next section, but in the meantime, calculation of gravitational effects can be performed with this library as follows:
"""
Gravitational Effects that affect the robot
"""
# Dynamics library
from lib.dynamics.DynamicsHTM import *
from lib.dynamics.DynamicsDQ import *
# Derivative of Potential Energy (with respect to "q" or joints positions): G(q)
G = gravitationalCOM(robot, g = np.array([[0], [0], [-9.80665]]), symbolic = False)
Gdq = dqGravitationalCOM(robot, g = np.array([[0], [0], [0], [-9.80665]]), symbolic = False)So the output will be
# NumPy Array
>>> G
array([[ 0. ],
[-5.26452724],
[ 0.41728099],
[ 0. ]])
>>> Gdq
array([[ 0. ],
[-5.26452724],
[ 0.41728099],
[ 0. ]])You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
This is only a linear transformation of the matrix shown in the previous section. To calculate it, is necessary to calculate the Geometric Jacobian Matrix of the robot and then multiply it as follows
The algorithm in this library calculates the Gravitational Effects in Joints Space automatically when you call it 😉
"""
Robot's Gravitational Effects in Cartesian Space
"""
# Dynamics library
from lib.dynamics.DynamicsHTM import *
# Gravitational Effects in Cartesian Space: G_c(q)
G_c = gravitationalCartesian(uRobot, symbolic = False)So the output will be
# NumPy Array
>>> G_c
array([[ 2.41440980],
[ 2.44731763],
[-3.96818515],
[-0.59666383],
[ 0.58864080],
[ 0. ]])You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
As part of the Euler - Lagrange formulation, it is mandatory to calculate the rate of change of the lagrangian with respect to the joints velocities, so this leads to
Using the equation shown in this section, is possible to expand previous result as
;
In previous result, the time derivatives are not constant, so each term has to be analyzed separately:
,
then the result can be mulitiplied by ones as follows
.
Simplification of previous result leads to
,
that represents the robot's dynamic model. is the matrix with the coriolis and centrifugal effects based on each joint position and velocity
. Its representation can be simplified by considering the dyadic expansion
,
where is the i - th column of inertia matrix; also,
is an auxiliar vector to map current column to a matrix. For example, second column (
) of inertia matrix
is analyzed, so
,
,
so derivative can be rewriten as
where each is a square matrix. With this result is possible to simplify
:
,
that is easier to calculate compared with the original result. It can be performed with this library as follows:
"""
Centrifugal and Coriolis Effects that affect the robot
"""
# Dynamics library
from lib.dynamics.DynamicsHTM import *
from lib.dynamics.DynamicsDQ import *
# Centrifugal and Coriolis Matrix: C(q, q')
C = centrifugalCoriolisCOM(uRobot, symbolic = False)
Cdq = dqCentrifugalCoriolisCOM(uRobot, symbolic = False)So the output will be
# NumPy Array
>>> C
array([[-0.0956919 , 1.02659223, 0.41980575, -0.11168224],
[-0.78420819, -0.1879901 , -0.12435985, 0.58833922],
[ 0.0373449 , 0.00698386, -0.06573144, 0.58833922],
[-0.18311305, -0.303166 , -0.303166 , -0.08189253]])
>>> Cdq
array([[-0.0956919 , 1.02659223, 0.41980575, -0.11168224],
[-0.78420819, -0.1879901 , -0.12435985, 0.58833922],
[ 0.0373449 , 0.00698386, -0.06573144, 0.58833922],
[-0.18311305, -0.303166 , -0.303166 , -0.08189253]])You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
This is only a linear transformation of the matrix shown in the previous section. To calculate it, is necessary to calculate the Geometric Jacobian Matrix of the robot and its time derivative, and then multiply it as follows
The algorithm in this library calculates the Centrifugal and Coriolis Effects in Joints Space automatically when you call it 😉
"""
Robot's Centrifugal and Coriolis Effects in Cartesian Space
"""
# Dynamics library
from lib.dynamics.DynamicsHTM import *
# Centrifugal and Coriolis Effects in Cartesian Space: N(q, q')
N = centrifugalCoriolisCartesian(uRobot, symbolic = False)So the output will be
# NumPy Array
>>> N
array([[-1.71740759e+02, -1.28111703e+02, 6.25778875e+01, -1.15033719e+02, 1.73976636e+02, 3.92648627e+01],
[-1.81816062e+02, -1.37677613e+02, 7.48725450e+01, -1.15854383e+02, 1.75796505e+02, 3.96973201e+01],
[ 2.49125285e+02, 1.87702765e+02, -9.69125713e+01, 1.63257299e+02, -2.46348927e+02, -5.53672285e+01],
[-1.29239467e+00, -1.50100120e+00, 6.05246596e+00, 4.27684942e+00, -3.89963402e+00, -5.93160320e-01],
[-9.06390051e+00, -8.17392809e+00, 9.19365695e+00, -1.91862554e+00, 3.57394877e+00, 6.03999742e-01],
[-6.48484237e+00, -7.15739185e+00, 9.69076188e+00, -3.53936516e-01, -8.11586830e-01, -2.03684464e-01]])You can also calculate its symbolic expression by setting symbolic parameter to True, but this may be slow
Given the equation
,
it is possible to build the robot dynamic model by calculating each term individually and then build the complete model. This can be done as follows:
"""
Robot Dynamic Model in Joints Space
"""
# Dynamics library
from lib.dynamics.DynamicsHTM import *
from lib.dynamics.DynamicsDQ import *
# Inertia Matrix for Kinetic Energy equation: D(q)
D = inertiaMatrixCOM(uRobot, symbolic = False)
Ddq = dqInertiaMatrixCOM(uRobot, symbolic = False)
# Centrifugal and Coriolis Matrix: C(q, q')
C = centrifugalCoriolisCOM(uRobot, symbolic = False)
Cdq = dqCentrifugalCoriolisCOM(uRobot, symbolic = False)
# Gravitational Effects: G(q)
G = gravitationalCOM(uRobot, g = np.array([[0], [0], [-9.80665]]), symbolic = False)
Gdq = dqGravitationalCOM(uRobot, g = np.array([[0], [0], [0], [-9.80665]]), symbolic = False)
# Robot Dynamic Equation: D(q) * q''(t) + C(q, q') * q'(t) + G(q) = T
T = (D * uRobot.qddSymbolic) + (C * uRobot.jointsVelocities) + G
Tdq = (Ddq * uRobot.qddSymbolic) + (Cdq * uRobot.jointsVelocities) + GdqSo the output will be
# NumPy Array
>>> T
Matrix([[2.19614724949073*qdd1 - 0.529809692293813*qdd2 - 0.361583479510541*qdd3 - 0.940364805482366*qdd4 - 0.501577443586108],
[ -0.529809692293813*qdd1 + 1.16842748717723*qdd2 + 0.391926283358894*qdd3 + 0.18369710671051*qdd4 - 8.16954552618308],
[-0.361583479510541*qdd1 + 0.391926283358894*qdd2 + 0.583065553244191*qdd3 + 0.18369710671051*qdd4 + 2.41913184137719],
[-0.940364805482366*qdd1 + 0.18369710671051*qdd2 + 0.18369710671051*qdd3 + 0.670906731541869*qdd4 - 0.160951111845721]])
>>> Tdq
Matrix([[2.19614724949073*qdd1 - 0.529809692293813*qdd2 - 0.361583479510541*qdd3 - 0.940364805482366*qdd4 - 0.501577443586108],
[ -0.529809692293813*qdd1 + 1.16842748717723*qdd2 + 0.391926283358894*qdd3 + 0.18369710671051*qdd4 - 8.16954552618308],
[-0.361583479510541*qdd1 + 0.391926283358894*qdd2 + 0.583065553244191*qdd3 + 0.18369710671051*qdd4 + 2.41913184137719],
[-0.940364805482366*qdd1 + 0.18369710671051*qdd2 + 0.18369710671051*qdd3 + 0.670906731541869*qdd4 - 0.160951111845721]])Please notice that this result is partially numerical because it includes joints accelerations () in symbolic form; they are stored in
uRobot.qddSymbolic and are the variables that have to be solved. If you need the numerical value, you can check this section to know how to do so. You can also calculate its full symbolic expression by setting symbolic parameter to True, but this may be slow
Given the equation
,
it is possible to build the robot dynamic model in cartesian space by calculating each term individually and then build the complete model. This can be done as follows:
"""
Robot Dynamic Model in Cartesian Space
"""
# Dynamics library
from lib.dynamics.DynamicsHTM import *
# Inertia Matrix in Cartesian Space: M(q)
M = inertiaMatrixCartesian(uRobot, symbolic = False)
# Centrifugal and Coriolis Effects in Cartesian Space: N(q, q')
N = centrifugalCoriolisCartesian(uRobot, symbolic = False)
# Gravitational Effects in Cartesian Space: G_c(q)
G_c = gravitationalCOM(uRobot, g = np.array([[0], [0], [-9.80665]]), symbolic = False)
# Robot Dynamic Equation: M(q) * x''(t) + N(q, q') * x'(t) + G(q) = F
F = (M * Matrix([['a_x'], ['a_y'], ['a_z'], ['dw_x'], ['dw_y'], ['dw_z']])) + (N * Matrix([['v_x'], ['v_y'], ['v_z'], ['w_x'], ['w_y'], ['w_z']])) + G_cSo the output will be
# NumPy Array
>>> F
Matrix([
[ 167.620328127733*a_x + 168.648739921281*a_y - 236.731217222324*a_z - 4.34024667414663*dw_x + 2.63005237014219*dw_y - 1.07386274939554*dw_z - 171.740759388159*v_x - 128.111702592246*v_y + 62.5778875499351*v_z - 115.033718691954*w_x + 173.976636371066*w_y + 39.2648627245533*w_z + 2.4144097975442],
[ 168.648739921281*a_x + 170.134620835683*a_y - 238.434092345566*a_z - 4.06495760517949*dw_x + 2.82298081033543*dw_y - 0.605471807570683*dw_z - 181.816061859663*v_x - 137.677613034654*v_y + 74.8725449524934*v_z - 115.854382542843*w_x + 175.796505187782*w_y + 39.6973200899513*w_z + 2.4473176251882],
[ -236.731217222324*a_x - 238.434092345566*a_y + 334.940578914865*a_z + 6.31943440887373*dw_x - 4.38037279321474*dw_y + 1.41511316050212*dw_z + 249.125285114767*v_x + 187.702765424408*v_y - 96.9125712754837*v_z + 163.257299237812*w_x - 246.34892664877*w_y - 55.3672285344599*w_z - 3.96818514532819],
[ -4.34024667414663*a_x - 4.06495760517949*a_y + 6.31943440887373*a_z + 0.518404749066483*dw_x - 0.458143257658009*dw_y + 0.576949172526721*dw_z - 1.29239467035411*v_x - 1.50100120452344*v_y + 6.0524659577958*v_z + 4.27684941977111*w_x - 3.89963402228553*w_y - 0.59316031983742*w_z - 0.596663833680832],
[ 2.63005237014219*a_x + 2.82298081033543*a_y - 4.38037279321473*a_z - 0.458143257658009*dw_x + 0.731741764371893*dw_y + 0.0791485515271751*dw_z - 9.06390051266166*v_x - 8.173928088836*v_y + 9.19365694823961*v_z - 1.91862554118406*w_x + 3.57394876644568*w_y + 0.603999741698912*w_z + 0.588640800463524],
[-1.07386274939555*a_x - 0.605471807570684*a_y + 1.41511316050213*a_z + 0.576949172526721*dw_x + 0.0791485515271751*dw_y + 0.402976991115265*dw_z - 6.48484236632503*v_x - 7.15739184563156*v_y + 9.69076188147293*v_z - 0.35393651566526*w_x - 0.811586829789977*w_y - 0.20368446392022*w_z - 2.63872720826792e-15]])Please notice that this result is partially numerical because it includes velocities and accelerations of the end - effector () in symbolic form; they are stored in
Matrix([['a_x'], ['a_y'], ['a_z'], ['dw_x'], ['dw_y'], ['dw_z']]) and Matrix([['v_x'], ['v_y'], ['v_z'], ['w_x'], ['w_y'], ['w_z']]), these are the variables that have to be solved. If you need the numerical value, you can check this section to know how to do so. You can also calculate its full symbolic expression by setting symbolic parameter to True, but this may be slow
We hope this can be useful for you. Thanks!
