Franka Emika Panda Matlab Simulation

Authors: Seonghyeon Jo(cpsc.seonghyeon@gmail.com)

Date: Des, 24, 2021

This repository is a MATLAB simulation of franka emika panda robot control using Runge-Kutta. The robot manipulator dynamic model (M, C, G, F) uses the functions provided in [1]. The kinematic function matches the actual panda robot data. However, Jacobian's derivative and Jacobian have some errors.

Function

real_data : Franka emika panda robot actual data files
model : Franka emika panda robot model library folder
- get_CoriolisMatrix.m : Coriolis matrix function $\C(\q,\dot{\q})$ [1]
- get_CoriolisVector.m : Coriolis vector function $\C(\q,\dot{\q})\dot{\q}$ [1]
- get_FrictionTorque.m : Friction torque function $\F(\dot{\q})$ [1]
- get_GravityVector.m : Gravity vector function $\G(\q)$ [1]
- get_MassMatrix.m : Inertia matrix function $\M(\q)$ [1]
- get_Jacobian_dot.m : Jacobian Differential Functions $\dot{\J}(\q,\dot{\q})$
- get_JacobianZ2YZ1.m : Jacobian function $\J(\q)$ (direction z2-y-z1)
- get_pose.m : kinematic function $\k(\q)$
- plant.m : robot manipulator plant function $\ddot{\q} = \M^{-1}(\q)(-\C(\q,\dot{\q})\dot{\q}-\G(\q)-\F(\dot{\q})+\boldsymbol\tau)$
- simple_plant.m : robot manipulator plant 함수 $\ddot{\q} = \M^{-1}(\q)(\boldsymbol\tau)$
- rk.m /simple_rk.m : Runge-Kutta 함수
1. test_model_kinematics.m : kinematics test code
1. test_model_jacobain.m : jacobian test Code
1. test_model_dot_jacobain.m : jacobian dot test Code
1. main_cartesian_pd_control_playback.m : Cartesian PD controller code
1. main_impedance_control.m : Cartesian Impedance Controller Code
1. main_simple_pd_control_playback.m : Simple PD controller code

Controller

simple pd controller

- catesian pd controller

- position based impendace controller

Kinematics Test

Franka Emika Robot DH parameters

i	1	2	3	4	5	6	7	8
theta	q1	q2	q3	q4	q5	q6	q7	0
d	0.333	0.000	0.316	0.000	0.384	0.000	0.000	0.107
a	0.000	0.000	0.000	0.0825	-0.0825	0.000	0.088	0.000
alpha	0	-pi/2	pi/2	pi/2	-pi/2	pi/2	pi/2	0

Modified DH parameters ${} ^{i-1} T_{i} = \text{rot}_{x,\alpha_{i-1}} \text{trans}_{x,a_{i-1}}\text{rot}_{z,\thata_{i}} \text{trans}_{z,d_{i}}$

${} ^{i-1} T_{i} = \left[{\begin{array}{ccc|c}\cos \theta _{i} & -\sin \theta _{i} & 0 & a_{i-1}\\ \sin \theta _{i}\cos \alpha _{i-1} & \cos \theta _{i}\cos \alpha _{i-1} & -\sin \alpha _{i-1} & -d_{i}\sin \alpha _{i-1}\\\sin \theta _{i}\sin \alpha _{i-1} & \cos \theta _{i}\sin \alpha _{i-1} & \cos \alpha _{i-1} & d_{i}\cos \alpha _{i-1}\\\hline 0 & 0 & 0 & 1\end{array}}\right]$

Ti= cell(n,1); 
for j=1:n
    if(j == 1)
        Ti{j} = Rhx(alpha(j)) * Rt(a(j), 0, 0)  * Rhz(q(j)) * Rt(0,0,d(j));
    else
        Ti{j} = Ti{j-1}*Rhx(alpha(j)) * Rt(a(j), 0, 0)  * Rhz(q(j)) * Rt(0,0,d(j));
   end
end
p = Ti{n}(1:3,4);
R = Ti{n}(1:3,1:3);

kinematics test plot

Jacobian Test

Jacobian test plot
Cartesian derivative(real) : $\dot{x} = J(q)\dot{q}$
- data1: $\dot{x} = (k(q)_{t+1}-k(q)_{t})\Delta t$
- data2: $\dot{x} = (x_{t+1}-x_{t})\Delta t$
- data3: $\dot{x} = \hat{J}(q)\dot{q}$
jacobain test plot

jacobian derivative

Jacobian derivative test plot
Cartesian second derivative(real) : $\ddot{x} = (\dot{x}_{t+1}-\dot{x}_{t})\Delta t$
- com: $\ddot{x} = J(q)\ddot{q} +\dot{J}(q,\dot{q})\dot{q}$
jacobain dot test plot