The post Easycat and SOEM appeared first on Youngmok Yun: Roboticist in The Univ. of Texas at Austin.

]]>This is still an ongoing project, and thus there are many insufficient parts.

The info about Easycat:

www.bausano.net/

The info about SOEM

https://openethercatsociety.github.io/

The code is attached here

http://youngmok.com/public_data/SOEM.zip

http://youngmok.com/public_data/TestEasyCat.zip

The below is the setup pic.

The below is an execution example.

reneu@reneu-Precision-T1600:~/soem/SOEM/build/test/linux/my_test$ sudo ./my_test eth0

SOEM (Simple Open EtherCAT Master)

Simple test

Starting simple test

ec_init succeeded.

5 slaves found and configured.

Slaves mapped, state to SAFE_OP.

Slave:1

Name:EK1100

Output size: 0bits

Input size: 0bits

State: 18

Delay: 0[ns]

Has DC: 1

Configured address: 1001

Outputs address: 0

Inputs address: 0

FMMUfunc 0:0 1:0 2:0 3:0

Slave:2

Name:EL1014

Output size: 0bits

Input size: 4bits

State: 18

Delay: 0[ns]

Has DC: 1

Configured address: 1002

Outputs address: 0

Inputs address: 8062180

FMMU0 Ls:40 Ll: 1 Lsb:0 Leb:3 Ps:1000 Psb:0 Ty:1 Act:1

FMMUfunc 0:2 1:0 2:0 3:0

Slave:3

Name:EL3102

Output size: 0bits

Input size: 48bits

State: 2

Delay: 0[ns]

Has DC: 1

Configured address: 1003

Outputs address: 0

Inputs address: 8062181

FMMU0 Ls:41 Ll: 6 Lsb:0 Leb:7 Ps:1180 Psb:0 Ty:1 Act:1

FMMUfunc 0:2 1:3 2:0 3:0

Slave:4

Name:EasyCAT 32+32

Output size: 256bits

Input size: 256bits

State: 18

Delay: 0[ns]

Has DC: 1

Configured address: 1004

Outputs address: 8062140

Inputs address: 8062187

FMMU0 Ls:0 Ll: 32 Lsb:0 Leb:7 Ps:1000 Psb:0 Ty:2 Act:1

FMMU1 Ls:47 Ll: 32 Lsb:0 Leb:7 Ps:1200 Psb:0 Ty:1 Act:1

FMMUfunc 0:1 1:2 2:0 3:0

Slave:5

Name:EasyCAT 32+32

Output size: 256bits

Input size: 256bits

State: 18

Delay: 0[ns]

Has DC: 1

Configured address: 1005

Outputs address: 8062160

Inputs address: 80621a7

FMMU0 Ls:20 Ll: 32 Lsb:0 Leb:7 Ps:1000 Psb:0 Ty:2 Act:1

FMMU1 Ls:67 Ll: 32 Lsb:0 Leb:7 Ps:1200 Psb:0 Ty:1 Act:1

FMMUfunc 0:1 1:2 2:0 3:0

Request operational state for all slaves

Operational state reached for all slaves.

0:

O: ff 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00

I: d4 d2 db 79 25 86 ab cd ef 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6c 94

1:

O: ff 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00

I: d4 d2 db 79 25 86 ab cd ef 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6c 94

2:

O: ff 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00

I: d3 cf db 79 25 86 ab cd ef 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6c 94

3:

O: ff 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00

I: d3 d1 db 79 25 86 ab cd ef 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6c 94

4:

O: ff 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00

I: d3 d1 db 79 25 86 ab cd ef 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6c 94

5:

O: ff 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00

I: d3 d1 db 79 25 86 ab cd ef 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6c 94

6:

O: ff 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00

I: d4 d1 db 79 25 86 ab cd ef 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 6c 94

7:

O: ff 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00

I: d3 d1 dc 79 24 86 ab cd ef 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 70 90

8:

O: ff 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00

I: d3 d1 dc 79 24 86 ab cd ef 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 70 90

9:

O: ff 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00

I: d3 d1 dc 79 24 86 ab cd ef 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 70 90

10:

O: ff 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00

I: d3 d2 dc 79 24 86 ab cd ef 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 70 90

The post Easycat and SOEM appeared first on Youngmok Yun: Roboticist in The Univ. of Texas at Austin.

]]>The post soldering two inline wires appeared first on Youngmok Yun: Roboticist in The Univ. of Texas at Austin.

]]>http://www.instructables.com/id/Soldering-wires-together/step4/Solder-the-splice-together/

The post soldering two inline wires appeared first on Youngmok Yun: Roboticist in The Univ. of Texas at Austin.

]]>The post Four bar mechanism made of paper appeared first on Youngmok Yun: Roboticist in The Univ. of Texas at Austin.

]]>The four-bar mechanism is very useful in the design of mechanical components. The best way to understand four-bar mechanisms is to build it by yourself. Here, I am introducing how to make a four-bar mechanism with only a paper, screws and nuts. It is very easy to follow.

If you want to know the theories of four-bar mechanisms, see the wiki. Here

One sheet of paper, four screws and nuts, and a nail (or any sharp one)

- Prepare a sheet of paper
- Tear the paper like this
- Fold two times
- Write numbers like the picture
- Make holes
- Now you should have four bars with eight holes
- Connect bars. Here the sequence is very important. If you do not follow this sequence the linkage will not rotate because interference with other linkages.
- Connect all
- Hold (4) and rotate (1)

This is the result.

This is a Grashof mechanism.

So far, I have introduced how to build a four-bar mechanism made of paper. I hope that this post helps your projects. If you have any question, please leave a reply.

The post Four bar mechanism made of paper appeared first on Youngmok Yun: Roboticist in The Univ. of Texas at Austin.

]]>The post How to install Opencv 3.1with extra modules appeared first on Youngmok Yun: Roboticist in The Univ. of Texas at Austin.

]]>http://milq.github.io/install-opencv-ubuntu-debian/

Then, refer to this site

http://embedonix.com/articles/image-processing/installing-opencv-3-1-0-on-ubuntu/

If “eigen” library makes a problem, copy the “unsupported” folder to the “user/include”

The post How to install Opencv 3.1with extra modules appeared first on Youngmok Yun: Roboticist in The Univ. of Texas at Austin.

]]>The post ADS7813 ADS7812 Arduino Code appeared first on Youngmok Yun: Roboticist in The Univ. of Texas at Austin.

]]>ADS7813/ADS7812 is a general purpose ADC made by Texas Instrument, and it is pretty easy to operate through SPI. You can see the datasheet from here.

This is the electric connection diagram that I used for my setup. I have used ADS7813 but all configurations are same with ADS7812. Just only difference is that the resolution of ADS7812 is 12bit .

This electric connection is actually a reference connection provided by TI datasheet. With this connection, I made the Arduino code like this.

#include <SPI.h> const int i_conv = 8; const int i_busy = 9; void setup() { Serial.begin(9600); // start the SPI library: SPI.begin(); pinMode(i_conv, OUTPUT); pinMode(i_busy, INPUT); delay(100); } void loop(){ bool i_busy_status=1; byte data1; byte data2; digitalWrite(i_conv,HIGH); delay(1); digitalWrite(i_conv,LOW); while ( digitalRead(i_busy) == LOW ){ } data1 = SPI.transfer(0x00); data2 = SPI.transfer(0x00); digitalWrite(i_conv,HIGH); Serial.print(data1); Serial.print(" "); Serial.print(data2); Serial.print("\n"); }

I included only very essential part so that you might be easily able to understand the code. I am also attaching the code. <Download>

This is the video showing the operation.

Nowadays I am playing with an Arduino, and I got many helps from communities. I hope this ADS7813 ADS7812 Arduino Code can help your project.

The post ADS7813 ADS7812 Arduino Code appeared first on Youngmok Yun: Roboticist in The Univ. of Texas at Austin.

]]>The post Matlab Code for Numerical Hessian Matrix appeared first on Youngmok Yun: Roboticist in The Univ. of Texas at Austin.

]]>In this post, I am sharing a **Malab code calculating a numerical hessian matrix. **

You can download here.

You can use **NumHessian.m** with this syntax.

**function hf = NumHessian(f,x0,varargin)**

You can understand how to use simply by reading these two simple examples.

>> NumHessian(@cos,0)

ans =

-1.0000

function y=test_func(x,a)

y=a*x(1)*x(2)*x(3);

>> NumHessian(@test_func,[1 2 3]’,2)

ans =

0 6.0000 4.0000

5.9999 -0.0001 1.9998

3.9999 2.0000 0

I hope this Matlab code for numerical Hessian matrix helps your projects

This Matlab code is based on another Matlab function, NumJacob, which calculates a numerical Jacobian matrix. If you are interested in this, visit here.

If you want to know the theory on Hessian matrix, please read this Wiki.

I acknowledge that this code is originally made by Parviz Khavari, a visitor of this blog, and I modified to post here.

The post Matlab Code for Numerical Hessian Matrix appeared first on Youngmok Yun: Roboticist in The Univ. of Texas at Austin.

]]>The post Compiling a Matlab C code with GCC/G++ appeared first on Youngmok Yun: Roboticist in The Univ. of Texas at Austin.

]]>I will use a very simple Matlab code, and the below is the code

function y = add_coder(a,b)

y=a+b

With the Matlab C coder, let’s auto-generate the C code. You have to specify the types of variables and some other options. If you just read the dialog box, it might not be difficult.

As a result, you can see this kind of result. If your Matlab version is not 2015, it might be little different, but its big flow is almost same.

This is my main.cpp file that is modified from the auto-generated main.cpp by Matlab C coder

#include "rt_nonfinite.h" #include "add_coder.h" #include "add_coder_terminate.h" #include "add_coder_initialize.h" #include "stdio.h" int main() { /* Initialize the application. You do not need to do this more than one time. */ add_coder_initialize(); /* Invoke the entry-point functions. You can call entry-point functions multiple times. */ double a=10; double b=20.0; double y; y = add_coder(a, b); printf("y: %lf\n",y); /* Terminate the application. You do not need to do this more than one time. */ add_coder_terminate(); return 0; }

I located this main.cpp file with other all codes.

Now let’s compile all codes. Just compile main.cpp with all other cpp files. like this.

I am attaching all related files as a zip file. <Download>

Isn’t it easy??!!!

Have a good day~~!

The post Compiling a Matlab C code with GCC/G++ appeared first on Youngmok Yun: Roboticist in The Univ. of Texas at Austin.

]]>The post Vertex angle from three points, Matlab code appeared first on Youngmok Yun: Roboticist in The Univ. of Texas at Austin.

]]>This is the code, and you can download this one from here <Download>

function [ang] = get_vertex_ang_from_three_points(p0,p1,p2)

% get the vertex angle at p0 from three points p0,p1,p2

% p0, p1,p2 are two dim vector

%

%%% example

% p0=[0;0];p1=[1;0];p2=[0;1];

% get_vertex_ang_from_three_points(p0,p1,p2)

% answer = 1.57 (45 deg)

%%%

v1 = p1-p0;

v2 = p2-p0;

ang = acos(v1′*v2/(norm(v1)*norm(v2)));

It is very easy to use. you can get the vertex angle from three points, It is written in Matlab code

Good luck~~

The post Vertex angle from three points, Matlab code appeared first on Youngmok Yun: Roboticist in The Univ. of Texas at Austin.

]]>The post Lagrange Equation by MATLAB with Examples appeared first on Youngmok Yun: Roboticist in The Univ. of Texas at Austin.

]]>In this post, I will explain how to derive a **dynamic equation **with **Lagrange Equation by MATLAB with Examples**. As an example, I will derive a **dynamic model **of a **three-DOF arm manipulator** (or **triple pendulum**). Of course you may get a dynamic model for a **two-DOF arm manipulator** by simply removing several lines. I am attaching demo codes for both two and three DOF arm manipulators.

If you know all theories and necessary skills and if you just want source code, you can just download from here.

In the attached file, “Symb_Development_3DOF.m” generates a dynamic model. “main_sim_three_dof_arm.m” runs a simulation. You may get a result like this.

Let’s suppose a three DOF arm manipulator shown in the below figure. I am assuming all of masses (M1, M2, M3) exist at the end of links for simplicity. Three actuators exist at each joint, and directly actuate the torques (u1,u2,u3). The manipulator kinematics is governed by three joint angles (q1, q2, q3).

We will get a dynamic equation of this system by using Lagrangian mechanics. If you do not have a background knowledge of Lagrangian mechanics, please refer here.

The general dynamic equation is obtained by

Where, T is the total kinetic energy, V is the total potential energy of the system. where is the generalized force, t is time, is the generalized coordinates, is the generalized velocity. For the example of three-DOF arm manipulator problem, is the torque at the j-th joint, is the angle of the j-th joint, is the angular velocity of the j-th joint.

I am using x to represent q, xd for , xdd for L is the length of each link. u is the torque of each joint. g is the gravity constant.

syms M1 M2 M3;

syms x1 x1d x1dd x2 x2d x2dd x3 x3d x3dd;

syms L1 L2 L3;

syms u1 u2 u3;

syms g

p1x = L1*cos(x1);**
p1y = L1*sin(x1);

p2x = p1x+L2

p2y = p1y+L2

p3x = p2x+L3

p3y = p2y+L3

v1x = -L1*sin(x1)*x1d;

v1y = +L1*cos(x1)*x1d;

v2x = v1x-L2*sin(x1+x2)*(x1d+x2d);

v2y = v1y+L2*cos(x1+x2)*(x1d+x2d);

v3x = v2x – L3*sin(x1+x2+x3)*(x1d+x2d+x3d);

v3y = v2y + L3*cos(x1+x2+x3)*(x1d+x2d+x3d);

KE = 0.5*M1*( v1x^2 + v1y^2) + 0.5*M2*( v2x^2 + v2y^2) + 0.5*M3*( v3x^2 + v3y^2);

KE = simplify(KE);

PE = M1*g*p1y + M2*g*p2y + M3*g*p3y;

PE = simplify(PE);

Px1 = u1;

Px2 = u2;

Px3 = u3;

We have to get the Lagrangian eqn.

Let’s obtain step by step.

is obtained by

pKEpx1d = diff(KE,x1d);

To calculate, , we need the chain rule, that is

ddtpKEpx1d = diff(pKEpx1d,x1)*x1d+ …**
diff(pKEpx1d,x1d)*x1dd+ …

diff(pKEpx1d,x2)

diff(pKEpx1d,x2d)

diff(pKEpx1d,x3)

diff(pKEpx1d,x3d)

are easily obtained by

pKEpx1 = diff(KE,x1);

pPEpx1 = diff(PE,x1);

By summing all equations,

eqx1 = simplify( ddtpKEpx1d – pKEpx1 + pPEpx1 – Px1);

By repeating these procedures, we can get all governing equations.

We love more simplified forms like

For this form, we need to rearrange the equations by

Sol = solve(eqx1,eqx2,eqx3,’x1dd,x2dd,x3dd’);

Sol.x1dd = simplify(Sol.x1dd);

Sol.x2dd = simplify(Sol.x2dd);

Sol.x3dd = simplify(Sol.x3dd);

Just for easier implementation of symbolic codes, let’s substitute x1, x1d, x2, … with y1,y2….

syms y1 y2 y3 y4 y5 y6

fx1=subs(Sol.x1dd,{x1,x1d,x2,x2d,x3,x3d},{y1,y2,y3,y4,y5,y6})

fx2=subs(Sol.x2dd,{x1,x1d,x2,x2d,x3,x3d},{y1,y2,y3,y4,y5,y6})

fx3=subs(Sol.x3dd,{x1,x1d,x2,x2d,x3,x3d},{y1,y2,y3,y4,y5,y6})

fx1 =

(L2*L3*M2*u1 – L2*L3*M2*u2 + L2*L3*M3*u1 – L2*L3*M3*u2 – L1*L3*M2*u2*cos(y3) + L1*L3*M2*u3*cos(y3) – L1*L3*M3*u2*cos(y3) + L1*L3*M3*u3*cos(y3) – L2*L3*M3*u1*cos(y5)^2 + L2*L3*M3*u2*cos(y5)^2 + L1*L2^2*L3*M2^2*y2^2*sin(y3) + L1*L2^2*L3*M2^2*y4^2*sin(y3) – L1*L2*L3*M2^2*g*cos(y1) + (L1^2*L2*L3*M2^2*y2^2*sin(2*y3))/2 + L1*L2*M2*u3*sin(y3)*sin(y5) + L1*L2*M3*u3*sin(y3)*sin(y5) + L1*L3*M3*u2*cos(y3)*cos(y5)^2 – L1*L3*M3*u3*cos(y3)*cos(y5)^2 + L1*L2^2*L3*M2*M3*y2^2*sin(y3) + L1*L2^2*L3*M2*M3*y4^2*sin(y3) – L1*L2*L3*M1*M2*g*cos(y1) – L1*L2*L3*M1*M3*g*cos(y1) – L1*L2*L3*M2*M3*g*cos(y1) + 2*L1*L2^2*L3*M2^2*y2*y4*sin(y3) + (L1^2*L2*L3*M2*M3*y2^2*sin(2*y3))/2 – L1*L3*M3*u2*cos(y5)*sin(y3)*sin(y5) + L1*L3*M3*u3*cos(y5)*sin(y3)*sin(y5) + L1*L2*L3*M2^2*g*cos(y1)*cos(y3)^2 – L1*L2*L3*M2^2*g*cos(y3)*sin(y1)*sin(y3) + 2*L1*L2^2*L3*M2*M3*y2*y4*sin(y3) + L1*L2*L3*M2*M3*g*cos(y1)*cos(y3)^2 + L1*L2*L3*M1*M3*g*cos(y1)*cos(y5)^2 + L1*L2*L3^2*M2*M3*y2^2*cos(y5)*sin(y3) + L1*L2*L3^2*M2*M3*y4^2*cos(y5)*sin(y3) + L1*L2*L3^2*M2*M3*y6^2*cos(y5)*sin(y3) + 2*L1*L2*L3^2*M2*M3*y2*y4*cos(y5)*sin(y3) + 2*L1*L2*L3^2*M2*M3*y2*y6*cos(y5)*sin(y3) + 2*L1*L2*L3^2*M2*M3*y4*y6*cos(y5)*sin(y3) – L1*L2*L3*M2*M3*g*cos(y3)*sin(y1)*sin(y3))/(L1^2*L2*L3*(M2^2 – M2^2*cos(y3)^2 + M1*M2 + M1*M3 + M2*M3 – M2*M3*cos(y3)^2 – M1*M3*cos(y5)^2))

fx2 =

-(2*L1^2*L3*M1*u3 – 2*L1^2*L3*M1*u2 – 2*L1^2*L3*M2*u2 + 2*L2^2*L3*M2*u1 + 2*L1^2*L3*M2*u3 – L1^2*L3*M3*u2 – 2*L2^2*L3*M2*u2 + L2^2*L3*M3*u1 + L1^2*L3*M3*u3 – L2^2*L3*M3*u2 + L1*L2^2*M2*u3*cos(y3 – y5) + L1*L2^2*M3*u3*cos(y3 – y5) – L1^2*L2*M2*u3*cos(2*y3 + y5) – L1^2*L2*M3*u3*cos(2*y3 + y5) – L2^2*L3*M3*u1*cos(2*y5) + L2^2*L3*M3*u2*cos(2*y5) + L1^2*L3*M3*u2*cos(2*y3 + 2*y5) – L1^2*L3*M3*u3*cos(2*y3 + 2*y5) – L1*L2^2*M2*u3*cos(y3 + y5) – L1*L2^2*M3*u3*cos(y3 + y5) + 2*L1^2*L2*M1*u3*cos(y5) + L1^2*L2*M2*u3*cos(y5) + L1^2*L2*M3*u3*cos(y5) + 2*L1*L2^3*L3*M2^2*y2^2*sin(y3) + 2*L1^3*L2*L3*M2^2*y2^2*sin(y3) + 2*L1*L2^3*L3*M2^2*y4^2*sin(y3) – L1*L2*L3*M3*u1*cos(y3 + 2*y5) + 2*L1*L2*L3*M3*u2*cos(y3 + 2*y5) – L1*L2*L3*M3*u3*cos(y3 + 2*y5) + L1^2*L2*L3*M2^2*g*cos(y1 + y3) – L1*L2^2*L3*M2^2*g*cos(y1) – L1^2*L2*L3*M2^2*g*cos(y1 – y3) + L1*L2^2*L3*M2^2*g*cos(y1 + 2*y3) + 2*L1^2*L2^2*L3*M2^2*y2^2*sin(2*y3) + L1^2*L2^2*L3*M2^2*y4^2*sin(2*y3) + 2*L1*L2*L3*M2*u1*cos(y3) – 4*L1*L2*L3*M2*u2*cos(y3) + L1*L2*L3*M3*u1*cos(y3) + 2*L1*L2*L3*M2*u3*cos(y3) – 2*L1*L2*L3*M3*u2*cos(y3) + L1*L2*L3*M3*u3*cos(y3) + 2*L1^3*L2*L3*M1*M2*y2^2*sin(y3) + L1^3*L2*L3*M1*M3*y2^2*sin(y3) + 2*L1*L2^3*L3*M2*M3*y2^2*sin(y3) + 2*L1^3*L2*L3*M2*M3*y2^2*sin(y3) + 2*L1*L2^3*L3*M2*M3*y4^2*sin(y3) + 4*L1*L2^3*L3*M2^2*y2*y4*sin(y3) – (L1^2*L2*L3*M1*M3*g*cos(y1 + y3 + 2*y5))/2 – L1^3*L2*L3*M1*M3*y2^2*sin(y3 + 2*y5) + L1*L2^2*L3^2*M2*M3*y2^2*sin(y3 + y5) + L1*L2^2*L3^2*M2*M3*y4^2*sin(y3 + y5) + L1*L2^2*L3^2*M2*M3*y6^2*sin(y3 + y5) + L1^2*L2*L3*M1*M2*g*cos(y1 + y3) + (L1^2*L2*L3*M1*M3*g*cos(y1 + y3))/2 + L1^2*L2*L3*M2*M3*g*cos(y1 + y3) – 2*L1^2*L2*L3^2*M1*M3*y2^2*sin(y5) – L1^2*L2*L3^2*M2*M3*y2^2*sin(y5) – 2*L1^2*L2*L3^2*M1*M3*y4^2*sin(y5) – L1^2*L2*L3^2*M2*M3*y4^2*sin(y5) – 2*L1^2*L2*L3^2*M1*M3*y6^2*sin(y5) – L1^2*L2*L3^2*M2*M3*y6^2*sin(y5) – 2*L1*L2^2*L3*M1*M2*g*cos(y1) – L1*L2^2*L3*M1*M3*g*cos(y1) – L1*L2^2*L3*M2*M3*g*cos(y1) + (L1^2*L2*L3*M1*M3*g*cos(y1 – y3 – 2*y5))/2 + L1*L2^2*L3^2*M2*M3*y2^2*sin(y3 – y5) + L1^2*L2*L3^2*M2*M3*y2^2*sin(2*y3 + y5) + L1*L2^2*L3^2*M2*M3*y4^2*sin(y3 – y5) + L1^2*L2*L3^2*M2*M3*y4^2*sin(2*y3 + y5) + L1*L2^2*L3^2*M2*M3*y6^2*sin(y3 – y5) + L1^2*L2*L3^2*M2*M3*y6^2*sin(2*y3 + y5) – L1^2*L2*L3*M1*M2*g*cos(y1 – y3) – (L1^2*L2*L3*M1*M3*g*cos(y1 – y3))/2 – L1^2*L2*L3*M2*M3*g*cos(y1 – y3) + L1*L2^2*L3*M2*M3*g*cos(y1 + 2*y3) + (L1*L2^2*L3*M1*M3*g*cos(y1 – 2*y5))/2 + (L1*L2^2*L3*M1*M3*g*cos(y1 + 2*y5))/2 + 2*L1^2*L2^2*L3*M2*M3*y2^2*sin(2*y3) – L1^2*L2^2*L3*M1*M3*y2^2*sin(2*y5) + L1^2*L2^2*L3*M2*M3*y4^2*sin(2*y3) – L1^2*L2^2*L3*M1*M3*y4^2*sin(2*y5) + 2*L1^2*L2^2*L3*M2^2*y2*y4*sin(2*y3) + 4*L1*L2^3*L3*M2*M3*y2*y4*sin(y3) + 2*L1*L2^2*L3^2*M2*M3*y2*y4*sin(y3 + y5) + 2*L1*L2^2*L3^2*M2*M3*y2*y6*sin(y3 + y5) + 2*L1*L2^2*L3^2*M2*M3*y4*y6*sin(y3 + y5) – 4*L1^2*L2*L3^2*M1*M3*y2*y4*sin(y5) – 2*L1^2*L2*L3^2*M2*M3*y2*y4*sin(y5) – 4*L1^2*L2*L3^2*M1*M3*y2*y6*sin(y5) – 2*L1^2*L2*L3^2*M2*M3*y2*y6*sin(y5) – 4*L1^2*L2*L3^2*M1*M3*y4*y6*sin(y5) – 2*L1^2*L2*L3^2*M2*M3*y4*y6*sin(y5) + 2*L1*L2^2*L3^2*M2*M3*y2*y4*sin(y3 – y5) + 2*L1^2*L2*L3^2*M2*M3*y2*y4*sin(2*y3 + y5) + 2*L1*L2^2*L3^2*M2*M3*y2*y6*sin(y3 – y5) + 2*L1^2*L2*L3^2*M2*M3*y2*y6*sin(2*y3 + y5) + 2*L1*L2^2*L3^2*M2*M3*y4*y6*sin(y3 – y5) + 2*L1^2*L2*L3^2*M2*M3*y4*y6*sin(2*y3 + y5) + 2*L1^2*L2^2*L3*M2*M3*y2*y4*sin(2*y3) – 2*L1^2*L2^2*L3*M1*M3*y2*y4*sin(2*y5))/(L1^2*L2^2*L3*(M2^2 – M2^2*cos(2*y3) + 2*M1*M2 + M1*M3 + M2*M3 – M2*M3*cos(2*y3) – M1*M3*cos(2*y5)))

fx3 =

-(L1*L3^2*M3^2*u2 – L1*L2^2*M3^2*u3 – L1*L2^2*M2^2*u3 – L1*L3^2*M3^2*u3 – L2*L3^2*M3^2*u1*cos(y3) + L2*L3^2*M3^2*u2*cos(y3) – 2*L1*L2^2*M1*M2*u3 – 2*L1*L2^2*M1*M3*u3 + 2*L1*L3^2*M1*M3*u2 – 2*L1*L2^2*M2*M3*u3 – 2*L1*L3^2*M1*M3*u3 + 2*L1*L3^2*M2*M3*u2 – 2*L1*L3^2*M2*M3*u3 + L1*L2^2*M2^2*u3*cos(2*y3) + L1*L2^2*M3^2*u3*cos(2*y3) + L1*L3^2*M3^2*u2*sin(2*y3)*sin(2*y5) – L1*L3^2*M3^2*u3*sin(2*y3)*sin(2*y5) + L1*L2*L3*M3^2*u2*cos(y5) – 2*L1*L2*L3*M3^2*u3*cos(y5) – 2*L2*L3^2*M2*M3*u1*cos(y3) + 2*L2*L3^2*M2*M3*u2*cos(y3) – 2*L2^2*L3*M3^2*u1*sin(y3)*sin(y5) + 2*L2^2*L3*M3^2*u2*sin(y3)*sin(y5) + L2*L3^2*M3^2*u1*cos(2*y5)*cos(y3) – L2*L3^2*M3^2*u2*cos(2*y5)*cos(y3) + 2*L1*L2^2*M2*M3*u3*cos(2*y3) – L2*L3^2*M3^2*u1*sin(2*y5)*sin(y3) + L2*L3^2*M3^2*u2*sin(2*y5)*sin(y3) – L1*L3^2*M3^2*u2*cos(2*y3)*cos(2*y5) + L1*L3^2*M3^2*u3*cos(2*y3)*cos(2*y5) – L1*L2^2*L3^2*M2*M3^2*y2^2*sin(2*y3) – L1*L2^2*L3^2*M2^2*M3*y2^2*sin(2*y3) + 2*L1*L2^2*L3^2*M1*M3^2*y2^2*sin(2*y5) – L1*L2^2*L3^2*M2*M3^2*y4^2*sin(2*y3) – L1*L2^2*L3^2*M2^2*M3*y4^2*sin(2*y3) + 2*L1*L2^2*L3^2*M1*M3^2*y4^2*sin(2*y5) + L1*L2^2*L3^2*M1*M3^2*y6^2*sin(2*y5) + 2*L1*L2*L3*M1*M3*u2*cos(y5) – 4*L1*L2*L3*M1*M3*u3*cos(y5) + L1*L2*L3*M2*M3*u2*cos(y5) – 2*L1*L2*L3*M2*M3*u3*cos(y5) – 2*L2^2*L3*M2*M3*u1*sin(y3)*sin(y5) + 2*L2^2*L3*M2*M3*u2*sin(y3)*sin(y5) + 2*L1*L2*L3^3*M1*M3^2*y2^2*sin(y5) + 2*L1*L2^3*L3*M1*M3^2*y2^2*sin(y5) + L1*L2*L3^3*M2*M3^2*y2^2*sin(y5) + 2*L1*L2*L3^3*M1*M3^2*y4^2*sin(y5) + 2*L1*L2^3*L3*M1*M3^2*y4^2*sin(y5) + L1*L2*L3^3*M2*M3^2*y4^2*sin(y5) + 2*L1*L2*L3^3*M1*M3^2*y6^2*sin(y5) + L1*L2*L3^3*M2*M3^2*y6^2*sin(y5) – L1*L2*L3*M3^2*u2*cos(2*y3)*cos(y5) + 2*L1*L2*L3*M3^2*u3*cos(2*y3)*cos(y5) + L1*L2*L3*M3^2*u2*sin(2*y3)*sin(y5) – 2*L1*L2*L3*M3^2*u3*sin(2*y3)*sin(y5) – L1^2*L2*L3^2*M1*M3^2*y2^2*sin(y3) – 2*L1^2*L2*L3^2*M2*M3^2*y2^2*sin(y3) – 2*L1^2*L2*L3^2*M2^2*M3*y2^2*sin(y3) – 2*L1*L2^2*L3^2*M2*M3^2*y2*y4*sin(2*y3) – 2*L1*L2^2*L3^2*M2^2*M3*y2*y4*sin(2*y3) + 4*L1*L2^2*L3^2*M1*M3^2*y2*y4*sin(2*y5) + 2*L1*L2^2*L3^2*M1*M3^2*y2*y6*sin(2*y5) + 2*L1*L2^2*L3^2*M1*M3^2*y4*y6*sin(2*y5) + 2*L1*L2^3*L3*M1*M2*M3*y2^2*sin(y5) + 2*L1*L2^3*L3*M1*M2*M3*y4^2*sin(y5) – L1*L2*L3*M2*M3*u2*cos(2*y3)*cos(y5) + 2*L1*L2*L3*M2*M3*u3*cos(2*y3)*cos(y5) + 4*L1*L2*L3^3*M1*M3^2*y2*y4*sin(y5) + 4*L1*L2^3*L3*M1*M3^2*y2*y4*sin(y5) + 2*L1*L2*L3^3*M2*M3^2*y2*y4*sin(y5) + 4*L1*L2*L3^3*M1*M3^2*y2*y6*sin(y5) + 2*L1*L2*L3^3*M2*M3^2*y2*y6*sin(y5) + 4*L1*L2*L3^3*M1*M3^2*y4*y6*sin(y5) + 2*L1*L2*L3^3*M2*M3^2*y4*y6*sin(y5) + L1*L2*L3*M2*M3*u2*sin(2*y3)*sin(y5) – 2*L1*L2*L3*M2*M3*u3*sin(2*y3)*sin(y5) – L1*L2*L3^3*M2*M3^2*y2^2*cos(2*y3)*sin(y5) – L1*L2*L3^3*M2*M3^2*y2^2*sin(2*y3)*cos(y5) – L1*L2*L3^3*M2*M3^2*y4^2*cos(2*y3)*sin(y5) – L1*L2*L3^3*M2*M3^2*y4^2*sin(2*y3)*cos(y5) – L1*L2*L3^3*M2*M3^2*y6^2*cos(2*y3)*sin(y5) – L1*L2*L3^3*M2*M3^2*y6^2*sin(2*y3)*cos(y5) + 2*L1^2*L2^2*L3*M1*M3^2*y2^2*cos(y3)*sin(y5) – 2*L1^2*L2*L3^2*M1*M2*M3*y2^2*sin(y3) + L1*L2*L3^2*M1*M3^2*g*sin(y1)*sin(y3) + 2*L1*L2*L3^2*M2*M3^2*g*sin(y1)*sin(y3) + 2*L1*L2*L3^2*M2^2*M3*g*sin(y1)*sin(y3) + L1^2*L2*L3^2*M1*M3^2*y2^2*cos(2*y5)*sin(y3) + L1^2*L2*L3^2*M1*M3^2*y2^2*sin(2*y5)*cos(y3) – L1*L2*L3^2*M1*M3^2*g*cos(2*y5)*sin(y1)*sin(y3) – L1*L2*L3^2*M1*M3^2*g*sin(2*y5)*cos(y3)*sin(y1) + 4*L1*L2^3*L3*M1*M2*M3*y2*y4*sin(y5) + 2*L1^2*L2^2*L3*M1*M2*M3*y2^2*cos(y3)*sin(y5) – 2*L1*L2*L3^3*M2*M3^2*y2*y4*cos(2*y3)*sin(y5) – 2*L1*L2*L3^3*M2*M3^2*y2*y4*sin(2*y3)*cos(y5) – 2*L1*L2*L3^3*M2*M3^2*y2*y6*cos(2*y3)*sin(y5) – 2*L1*L2*L3^3*M2*M3^2*y2*y6*sin(2*y3)*cos(y5) – 2*L1*L2*L3^3*M2*M3^2*y4*y6*cos(2*y3)*sin(y5) – 2*L1*L2*L3^3*M2*M3^2*y4*y6*sin(2*y3)*cos(y5) – 2*L1*L2^2*L3*M1*M3^2*g*cos(y3)*sin(y1)*sin(y5) + 2*L1*L2*L3^2*M1*M2*M3*g*sin(y1)*sin(y3) – 2*L1*L2^2*L3*M1*M2*M3*g*cos(y3)*sin(y1)*sin(y5))/(L1*L2^2*L3^2*M3*(M2^2 – M2^2*cos(2*y3) + 2*M1*M2 + M1*M3 + M2*M3 – M2*M3*cos(2*y3) – M1*M3*cos(2*y5)))

copy the result and paste the symbols in a Matlab function. Then, run an ODE with the function.

A sample code is here.

In the attached file, “Symb_Development_3DOF.m” generates a dynamic model. “main_sim_three_dof_arm.m” runs a simulation. Then, you can get this result.

So far, I have explained how to derive a **Lagrange Equation by MATLAB with Examples.** I hope that this post helps your project and save your time. Please leave a message if you have any question.

Update on 02/21/2016

I updated some code and posting about typo. “simple” -> “simplify” There is no function “simple” Now all programs are running well.

-Mok-

Reference

[1] http://en.wikipedia.org/wiki/Lagrangian_mechanics

[3] http://www.mathworks.com/matlabcentral/fileexchange/23037-lagrange-s-equations

The post Lagrange Equation by MATLAB with Examples appeared first on Youngmok Yun: Roboticist in The Univ. of Texas at Austin.

]]>The post MATLAB code for Numerical calculation of Jacobian matrix for a complex number appeared first on Youngmok Yun: Roboticist in The Univ. of Texas at Austin.

]]>This code was developed by Sithan Kanna, based on my code. If you are just interested in Jacobian of a real number, please see this post.

http://youngmok.com/numerical-jacobian-matrix-matlab/

I really thank to Sithan Kanna for sharing this nice code. If you have a question on this code please send an email to sithankanna a-t gmail d-o-t com

>> h=@(x)[x(1) ; conj(x(1))]; % nonlinear equation

conjug = 0;

real = 0;

x_test = [0.5*1j];

H = NumJacob(h, x_test, conjug, real)

H =

1.0000

-0.0000

Have a nice day, and thank you for visiting my blog.

-Mok-

—————————————————————————————————

I am Youngmok Yun, and writing about robotics theories and my research.

My main site is http://youngmok.com, and Korean ver. is http://yunyoungmok.tistory.com.

—————————————————————————————————

The post MATLAB code for Numerical calculation of Jacobian matrix for a complex number appeared first on Youngmok Yun: Roboticist in The Univ. of Texas at Austin.

]]>