Added the new DQ_SerialManipulatorMDH() Class #35

[juanjqo]

Hi @mmmarinho,

I included the new DQ_SerialManipulatorMDH() in Python. This version requires the C++ implementation of the Pull request: Added the new DQ_SerialManipulatorMDH() Class #35

I implemented two examples using the FrankaEmikaPanda serial manipulator and the Stanford serial manipulator. Both robots are modeled using both classes (using DH and MDH).

Examples:

import os
import sys
import numpy as np
from math import pi
from numpy import linalg as LA

build_path = os.path.abspath(__file__+'/../..')+'/python/build'
sys.path.insert(0,build_path)
from dqrobotics import*
from dqrobotics.robot_modeling import DQ_SerialManipulatorDH
from dqrobotics.robot_modeling import DQ_SerialManipulatorMDH

def FrankaEmikaPandaDH():
    #Standard DH model
    franka_DH_theta = np.array([0, 0, 0, 0, 0, 0, 0])
    franka_DH_d = np.array([0.333, 0, 3.16e-1, 0, 3.84e-1, 0, 1.07e-1])
    franka_DH_a = np.array([0, 0, 8.25e-2, -8.25e-2, 0, 8.8e-2, 0])
    franka_DH_alpha = np.array([-pi/2, pi/2, pi/2, -pi/2, pi/2, pi/2, 0])
    franka_DH_type =  np.array([0,0,0,0,0,0,0])
    franka_DH_matrix = np.array([franka_DH_theta, franka_DH_d, franka_DH_a, franka_DH_alpha, franka_DH_type])
    frankaDH = DQ_SerialManipulatorDH(franka_DH_matrix)
    return frankaDH

def FrankaEmikaPandaMDH():

    # Modified DH model
    franka_DH_theta = np.array([0, 0, 0, 0, 0, 0, 0])
    franka_DH_d = np.array([0.333, 0, 3.16e-1, 0, 3.84e-1, 0, 0])
    franka_DH_a = np.array([0, 0, 0, 8.25e-2, -8.25e-2, 0, 8.8e-2])
    franka_DH_alpha = np.array([0, -pi / 2, pi / 2, pi / 2, -pi / 2, pi / 2, pi / 2])
    franka_DH_type =  np.array([0,0,0,0,0,0,0])
    franka_DH_matrix = np.array([franka_DH_theta, franka_DH_d, franka_DH_a, franka_DH_alpha, franka_DH_type])
    frankaMDH = DQ_SerialManipulatorMDH(franka_DH_matrix)
    frankaMDH.set_effector(1 + DQ.E * 0.5 * DQ.k * 1.07e-1)
    return frankaMDH

frankaDH = FrankaEmikaPandaDH()
frankaMDH = FrankaEmikaPandaMDH()
q = np.random.rand(7)
q_dot = np.random.rand(7)
xDH = frankaDH.fkm(q)
xMDH = frankaMDH.fkm(q)
Jdh = frankaDH.pose_jacobian(q)
Jmdh = frankaMDH.pose_jacobian(q)
Jdh_dot = frankaDH.pose_jacobian_derivative(q, q_dot)
Jmdh_dot = haminus8(frankaMDH.get_effector())@frankaMDH.pose_jacobian_derivative(q, q_dot)
print('Error in fkm')
print(xDH-xMDH)
print('Error in pose jacobian: ', LA.norm(Jdh-Jmdh, 'fro'))
#print(Jdh-Jmdh)
print('Error in pose jacobian derivative',LA.norm(Jdh_dot-Jmdh_dot, 'fro'))
#print(Jdh_dot-Jmdh_dot)

Results:

Error in fkm
0
Error in pose jacobian:  5.477744755553998e-16
Error in pose jacobian derivative 6.420696527768584e-16
Process finished with exit code 0

Example 2

import os
import sys
import numpy as np
from math import pi
from numpy import linalg as LA

build_path = os.path.abspath(__file__+'/../..')+'/python/build'
sys.path.insert(0,build_path)
from dqrobotics import*
from dqrobotics.robot_modeling import DQ_SerialManipulatorDH
from dqrobotics.robot_modeling import DQ_SerialManipulatorMDH

def StanfordManipulatorDH():
    #Standard DH model
    d2 = 0.4
    d3 = 0.1
    d6 = 0.3
    robot_DH_theta = np.array([0, 0, 0, 0, 0, 0])
    robot_DH_d = np.array([0,d2,d3,0,0,d6])
    robot_DH_a = np.array([0, 0, 0, 0 ,0 ,0])
    robot_DH_alpha = np.array([-pi/2, pi/2, 0,-pi/2,pi/2,0])
    robot_DH_type =  np.array([0,0,1,0,0,0])
    robot_DH_matrix = np.array([robot_DH_theta, robot_DH_d, robot_DH_a, robot_DH_alpha, robot_DH_type])
    robotDH = DQ_SerialManipulatorDH(robot_DH_matrix)
    return robotDH

def StanfordManipulatorMDH():
    # Modified DH model
    d2 = 0.4
    d3 = 0.1
    d6 = 0.3
    robot_DH_theta = np.array([0, 0, 0, 0, 0, 0])
    robot_DH_d = np.array([0,d2,d3,0,0,0])
    robot_DH_a = np.array([0, 0, 0, 0, 0, 0])
    robot_DH_alpha = np.array([0,-pi/2, pi/2, 0,-pi/2,pi/2])
    robot_DH_type =  np.array([0,0,1,0,0,0])
    robot_DH_matrix = np.array([robot_DH_theta, robot_DH_d, robot_DH_a, robot_DH_alpha, robot_DH_type])
    robotMDH = DQ_SerialManipulatorMDH(robot_DH_matrix)
    robotMDH.set_effector(1+DQ.E*0.5*DQ.k*d6)
    return robotMDH

robotDH = StanfordManipulatorDH()
robotMDH = StanfordManipulatorMDH()
q = np.random.rand(6)
q_dot = np.random.rand(6)
xDH = robotDH.fkm(q)
xMDH = robotMDH.fkm(q)
Jdh = robotDH.pose_jacobian(q)
Jmdh = robotMDH.pose_jacobian(q)
Jdh_dot = robotDH.pose_jacobian_derivative(q, q_dot)
Jmdh_dot = haminus8(robotMDH.get_effector())@robotMDH.pose_jacobian_derivative(q, q_dot)
print('Error in fkm')
print(xDH-xMDH)
print('Error in pose jacobian: ', LA.norm(Jdh-Jmdh, 'fro'))
#print(Jdh-Jmdh)
print('Error in pose jacobian derivative',LA.norm(Jdh_dot-Jmdh_dot, 'fro'))
#print(Jdh_dot-Jmdh_dot)

Results:

Error in fkm
0
Error in pose jacobian:  5.056949529391881e-16
Error in pose jacobian derivative 3.9384335993085515e-16

Process finished with exit code 0

[mmmarinho]

@juanjqo Thanks! Before we proceed to reviewing this pull-request, could you add some testing functionality for the new class in python-tests? It can be inspired on what you already did for MATLAB.

[juanjqo]

@mmmarinho sure! I'll go to work on it.

[juanjqo]

Hi @dqrobotics/developers,

@mmmarinho, I added the unit tests for the DQ_SerialManipulatorMDH class here.

Cheers,

Juancho