Skip to article frontmatterSkip to article content
%%capture
'''
(C) Copyright 2020-2025 Murilo Marques Marinho (murilomarinho@ieee.org)

     This file is licensed in the terms of the
     Attribution-NonCommercial-ShareAlike 4.0 International (CC BY-NC-SA 4.0)
     license.

 Derivative work of:
 https://github.com/dqrobotics/learning-dqrobotics-in-matlab/tree/master/robotic_manipulators
 Contributors to this file:
     Murilo Marques Marinho (murilomarinho@ieee.org)
'''

DQ6 Robot Control Basics using DQ Robotics - Part 2

I found an issue

Thank you! Please report it at https://github.com/MarinhoLab/OpenExecutableBooksRobotics/issues

Introduction

In the last lesson, I introduced you to the basics of kinematic modeling and kinematic control using a 1-DoF planar robot. The main point of that lesson was to teach you how to develop your robots from scratch, if needed.

Nonetheless, the DQ Robotics library has many of those functionalities built-in. In this lesson, you will learn how to model serial manipulators using the Denavit-Hartenberg parameters and how to calculate important Jacobians using DQ Robotics. You will also learn how to create a basic kinematic controller using DQ Robotics.

%%capture
%pip install dqrobotics dqrobotics-pyplot
%pip install dqrobotics dqrobotics-pyplot --break-system-packages
from math import sin, cos

import numpy as np

from dqrobotics import *
from dqrobotics_extensions.pyplot import plot

import matplotlib.pyplot as plt
plt.rcParams["animation.html"] = "jshtml" # Need to output animation's videos
import matplotlib.animation as anm
from functools import partial # Need to call functions correctly for matplotlib animations

Introduction

In the last lesson, I introduced you to the basics of kinematic modeling and kinematic control using a 1-DoF planar robot. The main point of that lesson was to teach you how to develop your robots from scratch, if needed.

Nonetheless, the DQ Robotics library has many of those functionalities built-in. In this lesson, you will learn how to model serial manipulators using the Denavit-Hartenberg parameters and how to calculate important Jacobians using DQ Robotics. You will also learn how to create a basic kinematic controller using DQ Robotics.

Notation

Keep these in mind (we will also use this notation when writting papers to conferences and journals):

  • h∈Hh\in \mathbb{H} : a quaternion. (Bold-face, lowercase character)
  • h‾∈H\underline{h} \in \mathcal{H} : a dual quaternion. (Bold-face, underlined, lowercase character)
  • p,t,⋯∈Hpp,t,\cdots \in {\mathbb{H}}_p : pure quaternions. They represent points, positions, and translations. They are quaternions for which Re(h)=0\textrm{Re}\left(h\right)=0 .
  • r∈S3r\in {\mathbb{S}}^3 : unit quaternions. They represent orientations and rotations. They are quaternions for which ∣∣h∣∣=1||h||=1 .
  • x‾∈\underline{x} \in S‾\underline{\mathcal{S}} : unit dual quaternions. They represent poses and pose transformations. They are dual quaternions for which ∣∣h∣∣=1||h||=1 .
  • l‾∈Hp∩\underline{l} \in {\mathcal{H}}_p \cap S‾\underline{{\mathcal{S}}} : a Plücker line.
  • π‾∈{P(π‾)∈Hp}∩S‾\underline{{{\pi }}} \in \left\lbrace P\left(\underline{{{\pi }}} \right)\in {\mathbb{H}}_p \right\rbrace \cap \underline{{\mathcal{S}}} : a plane.

Problem Definition

image_0.png

  1. Let the robot RR be a 3-DoF planar robot, as drawn in Fig.1.
  2. Let FW{\mathcal{F}}_W be the world-reference frame.
  3. Let x‾RW(t)≜x‾R{\underline{x} }_R^W (t)\triangleq {\underline{x} }_R ∈\in S‾\underline{\mathcal{S}} represent the pose of the end effector.
  4. Let RR be composed of three rotational joints that rotates about their z-axis, composed in the joint-space vector q(t)≜q=[θ1 θ2 θ3]Tq\left(t\right)\triangleq q={\left\lbrack \theta_1 ~\theta_2 ~\theta_3 \right\rbrack }^T with θ1(t)≜θ1,θ2(t)≜θ2,θ3(t)≜θ3∈R\theta_1 \left(t\right)\triangleq \theta_1 ,\theta_2 \left(t\right)\triangleq \theta_2 ,\theta_3 \left(t\right)\triangleq \theta_3 \in \mathbb{R} . The rotation of the reference frames of each joint coincide with the rotation of FW{\mathcal{F}}_W when θ1=θ2=θ3=0\theta_1 =\theta_2 =\theta_3 =0 . The length of the links are l1,l2,l3∈l_1 ,l_2 ,l_3 \in R+−{0}{\mathbb{R}}^+ -\lbrace 0\rbrace .
  5. Consider that we can freely control the joint vector qq .

Problems:

  1. Obtain the (pose) foward kinematic model of the robot RR using a set of DH parameters.
  2. Obtain the pose Jacobian, rotation Jacobian, and translation Jacobian of RR .
  3. Using 1. and 2., design a closed-loop pose controller, rotation controller, and translation controller.

Modeling serial robots using Denavit-Hartenberg Parameters

In the last lesson, you modelled a 2-DoF planar robot. As the number of DoF and the complexity of the robots increase, modeling them requires a more general, systematic, and scalable strategy. In this lesson we will show how serial manipulators are modeled using the Denavit-Hartenberg (DH) parameters. This is the standard methodology used in DQ robotics for modeling serial robots.

Forward Kinematic Model using DH parameters

For robots in 3D space, obtaining the robot’s pose transformation is the most generic form of FKM for the end effector. When using unit dual quaternions, retrieving the rotation, translation, etc from the pose is quite straighforward. So let us obtain the pose FKM of the robot RR using the DH parameters.

Before going into detail about the DH parameters, let x‾0W∈S‾{\underline{x} }_0^W \in \underline{{\mathcal{S}}} be the reference frame at the base of the robot. For convenience, it can coincide with the reference frame of the first joint of the robot.

The first joint enacts a pose transformation from the reference frame of the first joint to the reference frame of the second joint given by

x‾10(θ1)≜x‾10∈S‾{\underline{ x } }_1^0 \left(\theta_1 \right)\triangleq {\underline{ x } }_1^0 \in \underline{{\mathcal{S}}}
(1)

that depends on the joint value of the first joint.

Given that the 3-DoF planar robot has three joints, the robot can be modeled with three consecutive transformations

x‾R=x‾10x‾21x‾32,{\underline{ x } }_R ={\underline{ x } }_1^0 {\underline{ x } }_2^1 {\underline{ x } }_3^2 ,
(2)

where x‾21(θ2)≜x‾21∈S‾{\underline{x} }_2^1 \left(\theta_2 \right)\triangleq {\underline{x} }_2^1 \in \underline{{\mathcal{S}}} and x‾32(θ3)≜x‾32∈S‾{\underline{x} }_3^2 \left(\theta_3 \right)\triangleq {\underline{x} }_3^2 \in \underline{{\mathcal{S}}} . This sequence of transformation is a methodology that can be used for a serial manipulator with any number of joints.

The DH parameters provide a systematic way to calculate each individual joint transformation of any n-DoF serial manipulator. Each joint transformation, x‾ii−1(θi)≜x‾ii−1∈S‾{\underline{x} }_i^{i-1} \left(\theta_i \right)\triangleq {\underline{x} }_i^{i-1} \in \underline{{\mathcal{S}}} , with i=1,2,3...ni=1,2,3\ldotp \ldotp \ldotp n is composed of four intermediate transformations, as follows

x‾ii−1≜x‾i′i(θi)x‾i′′i′(di)x‾i′′′i′′(ai)x‾ii′′′(αi),{\underline{ x } }_i^{i-1} \triangleq {\underline{ x } }_{i^{\prime } }^i \left(\theta_i \right){\underline{ x } }_{i^{\prime \prime } }^{i^{\prime } } \left(d_i \right){\underline{ x } }_{i^{\prime \prime \prime } }^{i^{\prime \prime } } \left(a_i \right){\underline{ x } }_i^{i^{\prime \prime \prime } } \left(\alpha_i \right),
(3)

where the DH parameters, for each joint, are θi,di,ai,αi∈R\theta_i ,d_i ,a_i ,\alpha_i \in \mathbb{R} . Each of those parameters is related to one transformation. The first is the rotation of θi\theta_i about the z-axis of frame Fi−1{\mathcal{F}}_{i-1}

x‾i′i−1(θi)=cos⁡(θi2)+k^sin⁡(θi2),{\underline{ x } }_{i^{\prime } }^{i-1} \left(\theta_i \right)=\cos \left(\frac{\theta_i }{2}\right)+\hat{k} \sin \left(\frac{\theta_i }{2}\right),
(4)

the second is a translation of did_i about the z-axis of frame Fi′{\mathcal{F}}_{i^{\prime } } ,

x‾i′′i′(di)=1+ε12k^di,{\underline{ x } }_{i^{\prime \prime } }^{i^{\prime } } \left(d_i \right)=1+\varepsilon \frac{1}{2}\hat{k} d_i ,
(5)

the third is the translation of aia_i about the x-axis of frame Fi′′{\mathcal{F}}_{i^{\prime \prime } } ,

x‾i′′′i′′(ai)=1+ε12ı^ai,{\underline{ x } }_{i^{\prime \prime \prime } }^{i^{\prime \prime } } \left(a_i \right)=1+\varepsilon \frac{1}{2}\hat{\imath} a_i ,
(6)

the fourth, and last, is the rotation of αi\alpha_i about the x-axis of frame Fi′′′{\mathcal{F}}_{i^{\prime \prime \prime } }

x‾ii′′′(αi)=cos⁡(αi2)+ı^sin⁡(αi2).{\underline{ x } }_i^{i^{\prime \prime \prime } } \left(\alpha_i \right)=\cos \left(\frac{\alpha_i }{2}\right)+\hat{\imath} \sin \left(\frac{\alpha_i }{2}\right).
(7)

Back to our example, the following table summarizes the DH parameters of the 3-DoF planar robot.

image_1.png

DQ Robotics Example

Let us create a class representing the 3-DoF planar robot using DH parameters. The good news is that most of the hard work is handled by DQ Robotics, using the following class

from dqrobotics.robot_modeling import DQ_SerialManipulatorDH
help(DQ_SerialManipulatorDH)
Help on class DQ_SerialManipulatorDH in module dqrobotics._dqrobotics._robot_modeling:

class DQ_SerialManipulatorDH(DQ_SerialManipulator)
 |  Method resolution order:
 |      DQ_SerialManipulatorDH
 |      DQ_SerialManipulator
 |      DQ_Kinematics
 |      pybind11_builtins.pybind11_object
 |      builtins.object
 |
 |  Methods defined here:
 |
 |  __init__(...)
 |      __init__(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulatorDH, arg0: numpy.ndarray[numpy.float64[m, n]]) -> None
 |
 |  get_alphas(...)
 |      get_alphas(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulatorDH) -> numpy.ndarray[numpy.float64[m, 1]]
 |
 |      Retrieves the vector of alphas.
 |
 |  get_as(...)
 |      get_as(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulatorDH) -> numpy.ndarray[numpy.float64[m, 1]]
 |
 |      Retrieves the vector of as.
 |
 |  get_ds(...)
 |      get_ds(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulatorDH) -> numpy.ndarray[numpy.float64[m, 1]]
 |
 |      Retrieves the vector of ds.
 |
 |  get_thetas(...)
 |      get_thetas(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulatorDH) -> numpy.ndarray[numpy.float64[m, 1]]
 |
 |      Retrieves the vector of thetas.
 |
 |  get_types(...)
 |      get_types(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulatorDH) -> numpy.ndarray[numpy.float64[m, 1]]
 |
 |      Retrieves the vector of types.
 |
 |  raw_fkm(...)
 |      raw_fkm(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulatorDH, arg0: numpy.ndarray[numpy.float64[m, 1]], arg1: int) -> dqrobotics._dqrobotics.DQ
 |
 |      Retrieves the raw FKM.
 |
 |  raw_pose_jacobian(...)
 |      raw_pose_jacobian(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulatorDH, arg0: numpy.ndarray[numpy.float64[m, 1]], arg1: int) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Retrieves the raw pose Jacobian.
 |
 |  raw_pose_jacobian_derivative(...)
 |      raw_pose_jacobian_derivative(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulatorDH, arg0: numpy.ndarray[numpy.float64[m, 1]], arg1: numpy.ndarray[numpy.float64[m, 1]], arg2: int) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Retrieves the raw pose Jacobian derivative.
 |
 |  ----------------------------------------------------------------------
 |  Methods inherited from DQ_SerialManipulator:
 |
 |  fkm(...)
 |      fkm(*args, **kwargs)
 |      Overloaded function.
 |
 |      1. fkm(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulator, arg0: numpy.ndarray[numpy.float64[m, 1]]) -> dqrobotics._dqrobotics.DQ
 |
 |      Gets the fkm.
 |
 |      2. fkm(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulator, arg0: numpy.ndarray[numpy.float64[m, 1]], arg1: int) -> dqrobotics._dqrobotics.DQ
 |
 |      Gets the fkm.
 |
 |  get_dim_configuration_space(...)
 |      get_dim_configuration_space(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulator) -> int
 |
 |      Retrieves the number of links.
 |
 |  get_effector(...)
 |      get_effector(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulator) -> dqrobotics._dqrobotics.DQ
 |
 |      Retrieves the effector.
 |
 |  get_lower_q_dot_limit(...)
 |      get_lower_q_dot_limit(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulator) -> numpy.ndarray[numpy.float64[m, 1]]
 |
 |      Retrieves the lower limit for the joint velocities.
 |
 |  get_lower_q_limit(...)
 |      get_lower_q_limit(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulator) -> numpy.ndarray[numpy.float64[m, 1]]
 |
 |      Retrieves the lower limit for the joint values.
 |
 |  get_upper_q_dot_limit(...)
 |      get_upper_q_dot_limit(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulator) -> numpy.ndarray[numpy.float64[m, 1]]
 |
 |      Retrieves the upper limit for the joint velocities.
 |
 |  get_upper_q_limit(...)
 |      get_upper_q_limit(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulator) -> numpy.ndarray[numpy.float64[m, 1]]
 |
 |      Retrieves the upper limit for the joint values.
 |
 |  pose_jacobian(...)
 |      pose_jacobian(*args, **kwargs)
 |      Overloaded function.
 |
 |      1. pose_jacobian(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulator, arg0: numpy.ndarray[numpy.float64[m, 1]], arg1: int) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Returns the pose Jacobian
 |
 |      2. pose_jacobian(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulator, arg0: numpy.ndarray[numpy.float64[m, 1]]) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Returns the pose Jacobian
 |
 |  pose_jacobian_derivative(...)
 |      pose_jacobian_derivative(*args, **kwargs)
 |      Overloaded function.
 |
 |      1. pose_jacobian_derivative(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulator, arg0: numpy.ndarray[numpy.float64[m, 1]], arg1: numpy.ndarray[numpy.float64[m, 1]], arg2: int) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Returns the pose Jacobian derivative
 |
 |      2. pose_jacobian_derivative(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulator, arg0: numpy.ndarray[numpy.float64[m, 1]], arg1: numpy.ndarray[numpy.float64[m, 1]]) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Returns the pose Jacobian derivative
 |
 |  set_effector(...)
 |      set_effector(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulator, arg0: dqrobotics._dqrobotics.DQ) -> dqrobotics._dqrobotics.DQ
 |
 |      Sets the effector.
 |
 |  set_lower_q_dot_limit(...)
 |      set_lower_q_dot_limit(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulator, arg0: numpy.ndarray[numpy.float64[m, 1]]) -> None
 |
 |      Sets the lower limit for the joint velocities.
 |
 |  set_lower_q_limit(...)
 |      set_lower_q_limit(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulator, arg0: numpy.ndarray[numpy.float64[m, 1]]) -> None
 |
 |      Sets the lower limit for the joint values.
 |
 |  set_upper_q_dot_limit(...)
 |      set_upper_q_dot_limit(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulator, arg0: numpy.ndarray[numpy.float64[m, 1]]) -> None
 |
 |      Sets the upper limit for the joint velocities.
 |
 |  set_upper_q_limit(...)
 |      set_upper_q_limit(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulator, arg0: numpy.ndarray[numpy.float64[m, 1]]) -> None
 |
 |      Sets the upper limit for the joint values.
 |
 |  ----------------------------------------------------------------------
 |  Methods inherited from DQ_Kinematics:
 |
 |  get_base_frame(...)
 |      get_base_frame(self: dqrobotics._dqrobotics._robot_modeling.DQ_Kinematics) -> dqrobotics._dqrobotics.DQ
 |
 |      Returns the current base frame
 |
 |  get_reference_frame(...)
 |      get_reference_frame(self: dqrobotics._dqrobotics._robot_modeling.DQ_Kinematics) -> dqrobotics._dqrobotics.DQ
 |
 |      Returns the current reference frame
 |
 |  set_base_frame(...)
 |      set_base_frame(self: dqrobotics._dqrobotics._robot_modeling.DQ_Kinematics, arg0: dqrobotics._dqrobotics.DQ) -> None
 |
 |      Sets the base frame
 |
 |  set_reference_frame(...)
 |      set_reference_frame(self: dqrobotics._dqrobotics._robot_modeling.DQ_Kinematics, arg0: dqrobotics._dqrobotics.DQ) -> None
 |
 |      Sets the reference frame
 |
 |  ----------------------------------------------------------------------
 |  Static methods inherited from DQ_Kinematics:
 |
 |  distance_jacobian(...) method of builtins.PyCapsule instance
 |      distance_jacobian(arg0: numpy.ndarray[numpy.float64[m, n]], arg1: dqrobotics._dqrobotics.DQ) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Returns the distance Jacobian
 |
 |  distance_jacobian_derivative(...) method of builtins.PyCapsule instance
 |      distance_jacobian_derivative(arg0: numpy.ndarray[numpy.float64[m, n]], arg1: numpy.ndarray[numpy.float64[m, n]], arg2: dqrobotics._dqrobotics.DQ, arg3: numpy.ndarray[numpy.float64[m, 1]]) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Returns the distance Jacobian derivative
 |
 |  line_jacobian(...) method of builtins.PyCapsule instance
 |      line_jacobian(arg0: numpy.ndarray[numpy.float64[m, n]], arg1: dqrobotics._dqrobotics.DQ, arg2: dqrobotics._dqrobotics.DQ) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Returns the line Jacobian
 |
 |  line_jacobian_derivative(...) method of builtins.PyCapsule instance
 |      line_jacobian_derivative(arg0: numpy.ndarray[numpy.float64[m, n]], arg1: numpy.ndarray[numpy.float64[m, n]], arg2: dqrobotics._dqrobotics.DQ, arg3: dqrobotics._dqrobotics.DQ, arg4: numpy.ndarray[numpy.float64[m, 1]]) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Returns the line Jacobian derivative
 |
 |  line_segment_to_line_segment_distance_jacobian(...) method of builtins.PyCapsule instance
 |      line_segment_to_line_segment_distance_jacobian(arg0: numpy.ndarray[numpy.float64[m, n]], arg1: numpy.ndarray[numpy.float64[m, n]], arg2: numpy.ndarray[numpy.float64[m, n]], arg3: dqrobotics._dqrobotics.DQ, arg4: dqrobotics._dqrobotics.DQ, arg5: dqrobotics._dqrobotics.DQ, arg6: dqrobotics._dqrobotics.DQ, arg7: dqrobotics._dqrobotics.DQ, arg8: dqrobotics._dqrobotics.DQ) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Returns the line segment to line segment distance Jacobian
 |
 |  line_to_line_angle_jacobian(...) method of builtins.PyCapsule instance
 |      line_to_line_angle_jacobian(arg0: numpy.ndarray[numpy.float64[m, n]], arg1: dqrobotics._dqrobotics.DQ, arg2: dqrobotics._dqrobotics.DQ) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Returns the line to line angle Jacobian
 |
 |  line_to_line_angle_residual(...) method of builtins.PyCapsule instance
 |      line_to_line_angle_residual(arg0: dqrobotics._dqrobotics.DQ, arg1: dqrobotics._dqrobotics.DQ, arg2: dqrobotics._dqrobotics.DQ) -> float
 |
 |      Returns the line to line angle residual
 |
 |  line_to_line_distance_jacobian(...) method of builtins.PyCapsule instance
 |      line_to_line_distance_jacobian(arg0: numpy.ndarray[numpy.float64[m, n]], arg1: dqrobotics._dqrobotics.DQ, arg2: dqrobotics._dqrobotics.DQ) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Returns the robot line to line distance Jacobian
 |
 |  line_to_line_residual(...) method of builtins.PyCapsule instance
 |      line_to_line_residual(arg0: dqrobotics._dqrobotics.DQ, arg1: dqrobotics._dqrobotics.DQ, arg2: dqrobotics._dqrobotics.DQ) -> float
 |
 |      Returns the robot line to line residual
 |
 |  line_to_point_distance_jacobian(...) method of builtins.PyCapsule instance
 |      line_to_point_distance_jacobian(arg0: numpy.ndarray[numpy.float64[m, n]], arg1: dqrobotics._dqrobotics.DQ, arg2: dqrobotics._dqrobotics.DQ) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Returns the robot line to point distance Jacobian
 |
 |  line_to_point_residual(...) method of builtins.PyCapsule instance
 |      line_to_point_residual(arg0: dqrobotics._dqrobotics.DQ, arg1: dqrobotics._dqrobotics.DQ, arg2: dqrobotics._dqrobotics.DQ) -> float
 |
 |      Returns the robot line to point residual
 |
 |  plane_jacobian(...) method of builtins.PyCapsule instance
 |      plane_jacobian(arg0: numpy.ndarray[numpy.float64[m, n]], arg1: dqrobotics._dqrobotics.DQ, arg2: dqrobotics._dqrobotics.DQ) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Returns the plane Jacobian
 |
 |  plane_jacobian_derivative(...) method of builtins.PyCapsule instance
 |      plane_jacobian_derivative(arg0: numpy.ndarray[numpy.float64[m, n]], arg1: numpy.ndarray[numpy.float64[m, n]], arg2: dqrobotics._dqrobotics.DQ, arg3: dqrobotics._dqrobotics.DQ, arg4: numpy.ndarray[numpy.float64[m, 1]]) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Returns the plane Jacobian derivative
 |
 |  plane_to_point_distance_jacobian(...) method of builtins.PyCapsule instance
 |      plane_to_point_distance_jacobian(arg0: numpy.ndarray[numpy.float64[m, n]], arg1: dqrobotics._dqrobotics.DQ) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Returns the robot plane to point distance Jacobian
 |
 |  plane_to_point_residual(...) method of builtins.PyCapsule instance
 |      plane_to_point_residual(arg0: dqrobotics._dqrobotics.DQ, arg1: dqrobotics._dqrobotics.DQ) -> float
 |
 |      Returns the robot plane to point residual
 |
 |  point_to_line_distance_jacobian(...) method of builtins.PyCapsule instance
 |      point_to_line_distance_jacobian(arg0: numpy.ndarray[numpy.float64[m, n]], arg1: dqrobotics._dqrobotics.DQ, arg2: dqrobotics._dqrobotics.DQ) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Returns the robot point to line distance Jacobian
 |
 |  point_to_line_residual(...) method of builtins.PyCapsule instance
 |      point_to_line_residual(arg0: dqrobotics._dqrobotics.DQ, arg1: dqrobotics._dqrobotics.DQ, arg2: dqrobotics._dqrobotics.DQ) -> float
 |
 |      Returns the robot point to line residual
 |
 |  point_to_plane_distance_jacobian(...) method of builtins.PyCapsule instance
 |      point_to_plane_distance_jacobian(arg0: numpy.ndarray[numpy.float64[m, n]], arg1: dqrobotics._dqrobotics.DQ, arg2: dqrobotics._dqrobotics.DQ) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Returns the robot point to plane distance Jacobian
 |
 |  point_to_plane_residual(...) method of builtins.PyCapsule instance
 |      point_to_plane_residual(arg0: dqrobotics._dqrobotics.DQ, arg1: dqrobotics._dqrobotics.DQ) -> float
 |
 |      Returns the robot point to plane residual
 |
 |  point_to_point_distance_jacobian(...) method of builtins.PyCapsule instance
 |      point_to_point_distance_jacobian(arg0: numpy.ndarray[numpy.float64[m, n]], arg1: dqrobotics._dqrobotics.DQ, arg2: dqrobotics._dqrobotics.DQ) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Returns the robot point to point distance Jacobian
 |
 |  point_to_point_residual(...) method of builtins.PyCapsule instance
 |      point_to_point_residual(arg0: dqrobotics._dqrobotics.DQ, arg1: dqrobotics._dqrobotics.DQ, arg2: dqrobotics._dqrobotics.DQ) -> float
 |
 |      Returns the robot point to point residual
 |
 |  rotation_jacobian(...) method of builtins.PyCapsule instance
 |      rotation_jacobian(arg0: numpy.ndarray[numpy.float64[m, n]]) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Returns the rotation Jacobian
 |
 |  rotation_jacobian_derivative(...) method of builtins.PyCapsule instance
 |      rotation_jacobian_derivative(arg0: numpy.ndarray[numpy.float64[m, n]]) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Returns the rotation Jacobian derivative
 |
 |  translation_jacobian(...) method of builtins.PyCapsule instance
 |      translation_jacobian(arg0: numpy.ndarray[numpy.float64[m, n]], arg1: dqrobotics._dqrobotics.DQ) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Returns the translation Jacobian
 |
 |  translation_jacobian_derivative(...) method of builtins.PyCapsule instance
 |      translation_jacobian_derivative(arg0: numpy.ndarray[numpy.float64[m, n]], arg1: numpy.ndarray[numpy.float64[m, n]], arg2: dqrobotics._dqrobotics.DQ, arg3: numpy.ndarray[numpy.float64[m, 1]]) -> numpy.ndarray[numpy.float64[m, n]]
 |
 |      Returns the translation Jacobian derivative
 |
 |  ----------------------------------------------------------------------
 |  Static methods inherited from pybind11_builtins.pybind11_object:
 |
 |  __new__(*args, **kwargs) class method of pybind11_builtins.pybind11_object
 |      Create and return a new object.  See help(type) for accurate signature.

Let us represent our robot in the following way, for l1=l2=l3=1l_1 =l_2 =l_3 =1 .

from three_dof_planar_robot_dh import ThreeDofPlanarRobotDH
help(ThreeDofPlanarRobotDH.kinematics)
Help on function kinematics in module three_dof_planar_robot_dh:

kinematics()
    Returns the kinematics of the ThreeDoFPlanarRobot as DQ_SerialManipulatorDH.

Note that we use

## This isn't currently exposed in Python and will be fixed in the next release
# DQ_SerialManipulatorDH.JOINT_ROTATIONAL;

to define a rotational joint, so we do not explicilty define θ1,θ2,θ3\theta_1 ,\theta_2 ,\theta_3 in our model.

To calculate the forward kinematics model and plot the robot model, we can simply call the methods already available in the class, as follows

# Initial joint values [rad]
theta1 = -0.1
theta2 = 0.55
theta3 = -1.02

# Joint vector
q = [theta1, theta2, theta3]

# Instantiate the robot kinematics
three_dof_planar_robot = ThreeDofPlanarRobotDH.kinematics()

# Get the fkm, based on theta
x_r = three_dof_planar_robot.fkm(q)

# Plot the robot
plt.figure()
ax = plt.axes(projection='3d')
ax.set_proj_type('ortho')
ax.view_init(azim=0, elev=90) #https://stackoverflow.com/questions/33084853/set-matplotlib-view-to-be-normal-to-the-x-y-plane-in-python
plt.xlabel('x [m]')
plt.ylabel('y [m]')
ax.set_zticks([])
plot(three_dof_planar_robot, q=q)
<Figure size 640x480 with 1 Axes>

For more details about the methods, check the documentation of the class using

help(DQ_SerialManipulatorDH.fkm)
Help on instancemethod in module dqrobotics._dqrobotics._robot_modeling:

fkm(...)
    fkm(*args, **kwargs)
    Overloaded function.

    1. fkm(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulator, arg0: numpy.ndarray[numpy.float64[m, 1]]) -> dqrobotics._dqrobotics.DQ

    Gets the fkm.

    2. fkm(self: dqrobotics._dqrobotics._robot_modeling.DQ_SerialManipulator, arg0: numpy.ndarray[numpy.float64[m, 1]], arg1: int) -> dqrobotics._dqrobotics.DQ

    Gets the fkm.


help(plot)
Help on function plot in module dqrobotics_extensions.pyplot._pyplot:

plot(obj, **kwargs)
    An aggregator for all plot functions related to dqrobotics. Currently, supports `DQ` and `DQ_SerialManipulator`.

    Import it as follows:

        import dqrobotics_extensions.pyplot as dqp

    Before this can be used, please remember to initialise the plt Axes. Example:

        plt.figure()
        ax = plt.axes(projection='3d')
        dqp.plot(i_)
        plt.show()

    Plotting a unit DQ `x` (See internal function `pyplot._pyplot._plot_pose`):

        dqp.plot(x)

    Plotting a line DQ `l_dq` (See internal function `pyplot._pyplot._plot_line`):

        dqp.plot(l_dq, line=True)

    Plotting a plane DQ `pi_dq` (See internal function `pyplot._pyplot._plot_plane`):

        dqp.plot(pi_dq, plane=True)

    Plotting a `DQ_SerialManipulator` called `robot` at joint configurations `q` (See internal function `pyplot._pyplot._plot_serial_manipulator`):

        dqp.plot(robot, q=q)

    :param obj: The input to be plotted.
    :param kwargs: For arguments depending on the type of plot you need, see the description above.
    :raises RuntimeError: If the input instance `obj` has no meaning for function, or if the `obj` is not valid for the input options.

Differential Kinematics Model using DH parameters

Similar to how we calculated the the DKM in the last lesson, the DKM can be calculated for any set of DH parameters.

Pose Jacobian

Using the FKM, we find

vec8(x‾˙R)=∂(vec8(x‾R))∂qq˙,{\textrm{vec}}_8 \left({\dot{\underline{ x } } }_R \right)=\frac{\partial \left({\textrm{vec}}_8 \left({\underline{ x } }_R \right)\right)}{\partial q}\dot{q} ,
(8)

where Jx‾≜∂(vec8(x‾R))∂qJ_{{\underline{x} }} \triangleq \frac{\partial \left({\textrm{vec}}_8 \left({\underline{x} }_R \right)\right)}{\partial q} is the pose Jacobian. We do not need to worry about the details of how to calculate this for now. The details of how to calculate the Jacobian for any serial manipulator are described from Page 38 of

ADORNO, B. V., Two-arm manipulation: from manipulators to enhanced human-robot collaboration [Contribution à la manipulation à deux bras : des manipulateurs à la collaboration homme-robot], Université Montpellier 2, Montpellier, France, 2011. (pdf)

Rotation Jacobian

The goal of this section is to find the rotation Jacobian, JrJ_r , so that the following relation holds

vec4(r˙R)=Jrq˙.{\textrm{vec}}_4 \left({\dot{r} }_R \right)=J_r \dot{q} .
(9)

The rotation Jacobian is useful to control the rotation of the end effector and can be used to calculate many other Jacobians.

We can conveniently find the rotation Jacobian using the pose Jacobian. To do so, remember that the robot’s end-effector pose can be decomposed as follows

x‾R=rR+12εtRrR.{\underline{ x } }_R =r_R +\frac{1}{2}\varepsilon t_R r_R .
(10)

That means that the first-order time-derivative is

vec8(x‾˙R)=vec8(P(x‾˙R))+vec8(D(x‾˙R)){\textrm{vec}}_8 \left({\dot{\underline{ x } } }_R \right)={\textrm{vec}}_8 \left(P\left({\dot{\underline{ x } } }_R \right)\right)+{\textrm{vec}}_8 \left(D\left({\dot{\underline{ x } } }_R \right)\right)
(11)

that can be re-written as

Jx‾q˙=[JP(x‾)0]q˙+[0JD(x‾)]q˙J_{{\underline{ x } }} \dot{q} =\left\lbrack \begin{array}{c} J_{P\left({\underline{ x } }\right)} \\ 0 \end{array}\right\rbrack \dot{q} +\left\lbrack \begin{array}{c} 0\\ J_{D\left({\underline{ x } }\right)} \end{array}\right\rbrack \dot{q}
(12)

which means that the pose Jacobian can be decomposed as

Jx‾q˙=[JP(x‾)JD(x‾)]q˙.J_{{\underline{ x } }} \dot{q} =\left\lbrack \begin{array}{c} J_{P\left({\underline{ x } }\right)} \\ J_{D\left({\underline{ x } }\right)} \end{array}\right\rbrack \dot{q} .
(13)

Notice that

JP(x‾)q˙=vec4(r˙R)J_{P\left({\underline{ x } }\right)} \dot{q} ={\textrm{vec}}_4 \left({\dot{r} }_R \right)
(14)
JP(x‾)q˙=Jrq˙J_{P\left({\underline{ x } }\right)} \dot{q} =J_r \dot{q}
(15)

which means that the rotation Jacobian is Jr=JP(x‾)J_r =J_{P\left({\underline{x} }\right)} . That is, the rotational Jacobian is composed of the first four rows of the pose Jacobian.

Translation Jacobian

The goal of this section is to find the translation Jacobian, JtJ_t , so that the following relation holds

vec4(t˙R)=Jtq˙{\textrm{vec}}_4 \left({\dot{t} }_R \right)=J_t \dot{q}
(16)

The translation Jacobian is useful to control the translation of the end effector and can be used to calculate many other Jacobians. We can conveniently find the translation Jacobian using the pose Jacobian and the end-effector’s pose.

We start from the translation relation

tR=translation(x‾R)=2D(x‾R)P(x‾R)∗t_R =\textrm{translation}\left({\underline{ x } }_R \right)=2D\left({\underline{ x } }_R \right)P{\left({\underline{ x } }_R \right)}^*
(17)
t˙R=2[D(x‾˙R)P(x‾R)∗+D(x‾R)P(x‾˙R)∗]{\dot{t} }_R =2\left\lbrack D\left({\dot{\underline{ x } } }_R \right)P{\left({\underline{ x } }_R \right)}^* +D\left({\underline{ x } }_R \right)P{\left({\dot{\underline{ x } } }_R \right)}^* \right\rbrack
(18)
vec4(t˙R)=2[H4−(P(x‾R)∗)JD(x‾)+H4+(D(x‾R))C4JP(x‾)]q˙{\textrm{vec}}_4 \left({\dot{t} }_R \right)=2\left\lbrack \overset{-}{\underset{4}{H}} \left(P{\left({\underline{ x } }_R \right)}^* \right)J_{D\left({\underline{ x } }\right)} +\overset{+}{\underset{4}{H}} \left(D\left({\underline{ x } }_R \right)\right)C_4 J_{P\left({\underline{ x } }\right)} \right\rbrack \dot{q}
(19)

hence

Jt=2[H4−(P(x‾R)∗)JD(x‾)+H4+(D(x‾R))C4JP(x‾)].J_t =2\left\lbrack \overset{-}{\underset{4}{H}} \left(P{\left({\underline{ x } }_R \right)}^* \right)J_{D\left({\underline{ x } }\right)} +\overset{+}{\underset{4}{H}} \left(D\left({\underline{ x } }_R \right)\right)C_4 J_{P\left({\underline{ x } }\right)} \right\rbrack .
(20)

DQ Robotics Example

The pose Jacobian can be computed using the DQ_SerialManipulatorDH class as follows.

# Get the pose Jacobian
Jx = three_dof_planar_robot.pose_jacobian(q)

The rotation Jacobian and translation Jacobian can be calculated using methods of its DQ_Kinematics superclass. For instance, the rotation Jacobian can be obtained as

# Get the rotation Jacobian, based on the pose Jacobian
Jr = three_dof_planar_robot.rotation_jacobian(Jx)

and the translation Jacobian can be obtained as

# Get the end-effector's pose
x = three_dof_planar_robot.fkm(q)
# Get the translation Jacobian
Jt = three_dof_planar_robot.translation_jacobian(Jx,x)

Task-space position control

In the last lesson you were introduced to the basics of robot control using the inverse differential kinematics model.

Instead of building the controller from scratch, you can use controllers already available in DQ Robotics.

Pseudo-Inverse Controller

In the last lesson, we implemented a simple pseudo-inverse-based kinematic controller. Let us revisit this topic using DQ Robotics.

from dqrobotics.robot_control import DQ_PseudoinverseController
from dqrobotics.robot_control import ControlObjective

Preliminaries

Let us start with the initial conditions of the problem.

Define the sampling time and how many seconds of control we will simulate.

# Sampling time [s]
tau = 0.01
# Simulation time [s]
time_final = 1

Define the initial robot posture.

# Initial joint values [rad]
theta1 = -0.4
theta2 = 1.71
theta3 = 0.85
# Arrange the joint values in a column vector
q_init = [theta1, theta2, theta3]

Define the desired translation

# Desired translation components [m]
tx = 1.25
ty = 1.25
# Desired translation
td = tx*i_ + ty*j_

then, the desired rotation.

# Desired rotation component [rad]
gamma = 0.49
# Desired rotation
rd = cos(gamma/2.0) + k_*sin(gamma/2.0)

The desired pose will then be

# Desired pose
xd = rd + 0.5*E_*td*rd

We then instantiate the robot kinematics, as follows.

# Create robot
three_dof_planar_robot = ThreeDofPlanarRobotDH.kinematics()

Translation Controller

The basic syntax to instantiate a translation controller is as follows.

The robot will align the end-effector translation with the desired translation. The rotation is not controlled.

# Instanteate the controller
translation_controller = DQ_PseudoinverseController(three_dof_planar_robot)
translation_controller.set_control_objective(ControlObjective.Translation)
translation_controller.set_gain(5.0)
translation_controller.set_damping(0) # Damping was not explained yet, set it as 0 to use pinv()

# Translation controller loop.
q = q_init
stored_q = []
for time in np.arange(0, time_final + tau,tau):
    # Store q for posterior animation
    stored_q.append(q)

    # Get the next control signal [rad/s]
    u = translation_controller.compute_setpoint_control_signal(q, vec4(td))

    # Move the robot
    q = q + u * tau

Plot related code with reusable functions animate_robot and setup_plot.

# Animation function
def animate_robot(n, robot, stored_q, xd):
    plt.cla()
    plot(robot, q=stored_q[n])
    plot(xd)
    plt.xlabel('x [m]')
    plt.xlim([-0.3, 2])
    plt.ylabel('y [m]')
    plt.ylim([-2, 2])
    ax = plt.gca()
    ax.axes.zaxis.set_ticklabels([])
    plt.title(f'Translation control time={time} s out of {time_final} s')

def setup_plot():
    fig = plt.figure()
    ax = plt.axes(projection='3d')
    ax.set_proj_type('ortho')
    ax.view_init(azim=0, elev=90) #https://stackoverflow.com/questions/33084853/set-matplotlib-view-to-be-normal-to-the-x-y-plane-in-python
    return fig, ax

fig, ax = setup_plot()
anm.FuncAnimation(fig, 
                  partial(animate_robot, robot=three_dof_planar_robot, stored_q=stored_q, xd=xd), 
                  frames=len(stored_q))
Loading...

Rotation Controller

The basic syntax to instantiate a rotation controller is as follows.

The robot will align the end-effector rotation with the desired rotation. The translation is not controlled.

# Instantiate the controller
translation_controller = DQ_PseudoinverseController(three_dof_planar_robot)
translation_controller.set_control_objective(ControlObjective.Rotation)
translation_controller.set_gain(5.0)
translation_controller.set_damping(0) # Damping was not explained yet, set it as 0 to use pinv()

# Rotation controller loop.
q = q_init
stored_q = []
for time in np.arange(0, time_final + tau,tau):
    # Store q for posterior animation
    stored_q.append(q)
    
    # Get the next control signal [rad/s]
    u = translation_controller.compute_setpoint_control_signal(q, vec4(rd));

    # Move the robot
    q = q + u * tau

The behavior can be seen as follows.

# Plot
fig, ax = setup_plot()
anm.FuncAnimation(fig, 
                  partial(animate_robot, robot=three_dof_planar_robot, stored_q=stored_q, xd=xd), 
                  frames=len(stored_q))
Loading...

Pose Controller

The basic syntax to instantiate a pose controller is as follows.

The robot will align the end-effector pose with the desired pose. If the rotation and translation cannot be achieved simulatenously, the robot will balance rotation and translation error, according to the controller definitions.

# Instantiate the controller
translation_controller = DQ_PseudoinverseController(three_dof_planar_robot)
translation_controller.set_control_objective(ControlObjective.Pose)
translation_controller.set_gain(5.0)
translation_controller.set_damping(0) # Damping was not explained yet, set it as 0 to use pinv()

# Translation controller loop.
q = q_init
stored_q = []
for time in np.arange(0, time_final + tau,tau):
    # Store q for posterior animation
    stored_q.append(q)

    # Get the next control signal [rad/s]
    u = translation_controller.compute_setpoint_control_signal(q, vec8(xd))

    # Move the robot
    q = q + u * tau

The behavior can be seen as follows.

# Plot
fig, ax = setup_plot()
anm.FuncAnimation(fig, 
                  partial(animate_robot, robot=three_dof_planar_robot, stored_q=stored_q, xd=xd), 
                  frames=len(stored_q))
Loading...

Homework

(1) Following the format of ThreeDofPlanarRobotDH.py, create a class called NDofPlanarRobotDH.py as shown in the figure.

image_2.png

The class must

  1. Consider all lengths of the links as unitary. That is, l1=l2=⋯=ln=1l_1 =l_2 =\cdots =l_n =1 .
  2. Have a “kinematics” method that takes the desired number of DoFs as input and returns the corresponding DQ_SerialManipulatorDH instance.

(2) Consider the desired translation td=5ı^+2ȷ^t_d =5\hat{\imath} +2\hat{\jmath} , desired rotation rd=cos⁡(−π8)+k^sin⁡(−π8)r_d =\cos \left(-\frac{\pi }{8}\right)+\hat{k} \sin \left(-\frac{\pi }{8}\right) , and initial posture θi(0)=π8\theta_i (0)=\frac{\pi }{8} for i=1...7i=1...7 . Use the class you created in (1) to instantiate a 7-DoF planar robot.

  1. create a script called seven_dof_planar_robot_translation_control.m that implements a task-space translation controller using a DQ_PseudoinverseController. Control the 7-DoF planar robot to td.t_d .
  2. create a script called seven_dof_planar_robot_rotation_control.m that implements a task-space rotation controller using a DQ_PseudoinverseController. Control the 7-DoF planar robot to rd.r_d .
  3. create a script called seven_dof_planar_robot_pose_control.m that implements a task-space pose controller using a DQ_PseudoinverseController. Control the 7-DoF planar robot to x‾d=rd+12εtdrd{\underline{x} }_d =r_d +\frac{1}{2}\varepsilon t_d r_d .
[Advanced] DQ Robotics
DQ5 Robot Control Basics using DQ Robotics - Part 1
[Advanced] DQ Robotics
DQ7 Robot Control Basics using DQ Robotics - Part 3