This document is posted to help you gain knowledge. Please leave a comment to let me know what you think about it! Share it to your friends and learn new things together.
Transcript
امیرکبیردانشگاه صنعتي
)پلي تکنیک تهران(
مهندسي برقدانشکده
عنوان
درجه آزادی 7کنترل و شبیه سازی روبات Upper-Limb Powered Exoskeleton
7 .......................................................................................................................................................... حرکات عمومی 2.2
92 .............................................. درجه آزادی 7 اسکلت خارجيتحلیل سینماتیکي و طراحي 2
16 ...................................................................................................................................... سینماتیک مستقیم روبات 3.2 DH ............................................................... 16مفاصل و تعیین چارامترهای مختصات یمحورها یاختصاص ده 3.2.1
11 .............................................................................................. پیاده سازی حرکات عمومی دست در فضای مفاصل 3.2.2
11...................................................................................................................... بررسی مشکل تکینگی در روبات 3.3
simulink / matlab...................................................................... 35پیاده سازی معادالت در 5 26 ........................................................................................................................................................................ مقدمه 111
Simulink ........................................................................................................................ 26پروژه در یساز ادهیپ 1.2
27............................................................................................................. و مقدار مرجع روبات هیمولد مقدار اول 1.3
14.......................................................................................................... انبساط و انقباض مچ حرکتی ساز هیشب 116
17.............................................................................................. شانه یانبساط و انقباض افق حرکتی ساز هیشب 116
63 ............................................................................................................. چرخش کامل شانه حرکتی ساز هیشب 6.13
22 ........................................................................................................... ساخت انیمیشن روبات 7
64 ......................................................................................................................................... مراحل ساخت انیمیشن 117
79 .............................................................................................................................. نتیجه گیری 9
.ديآ یمبه دست ريبه صورت ز یدوران یجنبش یبه دست آمده در باال انرژ یبا توجه به سرعت ها حال
(6 )
1
2
T T TU U F F H H
ro U U U F F F H H HT
I I I
انرژی پتانسیل روبات 2.2
. با باشدیم نیکه تابع ارتفاع عضو از سط زم . است یتنها شامل بخش گرانش ساتمیسا لیپتانسا یانرژ
0نقاط ريتصاو 113توجه به شاکل 0 0
, , U F HP P P یدر راساتا 0X از سط نشاان دهنده ارتفاع آن عضو
:ميدار لیپتانس یانرژ یاساس برا نياست. بر ا زمین
(7 ) 0 0 0
, , ,U U X F F X H H XU g M P M P M P
0ازمنظور که
,U XP مولفه0x 0عبارت
UP .است
فصل چهارم: مدلسازی دينامیکی روبات
24
رانژگمعادله ال استخراج 2.2
بدست ريبصورت ز یو معادالت حرکت ستمیس نيالگرانژ ل،یو پتانسا یجنبشا یاساتخراج عبارات انرژ با
.نديآیم
(1 ) tr roL T U T T U
(1 ) 1 2 3 4 5 6 7 , , , , , , ,Td L L
u q t t t t t t tdt q q
. ديآیدر م ريبه صورت ز سیستم یکیناميمعادله د maple( در نرم افزار 1حل معادله ) با
(13 ) (q,q)q (q)M C G uq
از آنها کيکد مربوط به هر واند بدست آمده mapleمعادالت در نرم افزار یتمام است که یالزم به توض
آورده شده است. (1) وستیدر پ
Simulink/ matlabفصل پنجم : پیاده سازی معادالت در
21
5
پنجمفصل
Simulink/ matlabپیاده سازی معادالت در
Simulink/ matlabفصل پنجم : پیاده سازی معادالت در
26
مقدمه 5.9
اند با اساااتفاده از پکیج شااادهتولید mapleتوساااط کاه معاادالت ديناامیکی و ژاکوبین سااایساااتم
“CodeGeneration” افزار قابل تبديل به زبان اين نرمmatlab توان با بکارگیری اين دستور می. اسات
ها را در آن محیط پیگیری نمود. سازیسازی کرد و شبیهپیاده matlabهمه معادالت را در فضای
Simulinkپیاده سازی پروژه در 5.3
Simulinkشماتیک کلي پروژه در نرم افزار 925شکل
که بیانگر ) initial conditionو ref راديمقاست. 111مطابق شاکل simulinkسااختار کلی پروژه در
روباتمفاصل هايی از موقعیت و سرعت به همراه فیدبک( موقعیت اولیه و حرکت مطلوب روبات هساتند
ref بق ورودیامطحرکت روبات برای را گشااتاورهای الزمبلوک کنترلر . شااودوارد میکنترلر بلوک به
شود به منظور يکسان شدن شرايط اولیه داده می روبات کیبه مدل دينامیگشتاور که اين نمايدتأمین می
Simulink/ matlabفصل پنجم : پیاده سازی معادالت در
27
مدل دينامیکی روبات موقعیت خروجی .شودهر دو يکی فرض میشرايط اولیه refروبات با نقطه شاروع
های بلوک. در ادامه شوداست که برای مشاهده و کنترلر از آن استفاده میو سارعت مفاصال qمفاصال
شوند. بطور تفصیلی شرح داده می 1،1آمده در شکل
مرجع روباتمقدار مولد مقدار اولیه و 5.2
مدار داخلي زیر سیستم مولد مقدار اولیه روبات 325شکل
ای طراحی شده است کهبگونه اين زير سیستم باشد.می 211ساختار داخلی اين زير سیستم مطابق شکل
گانه دست انسان را 1روبات يکی از حرکات دهدمی command ورودیبه کاربر با توجه به مقداری که
در فصل دوم آورده شده با شاکل و محدوده حرکتی . توضایحات مربوط به هر يک از حرکاتبدهدانجام
آمده است. 213 در جدولاين حرکات برای انجام های روبات ضمن نحوه تغییر هر يک از پارامتراست. در
را انجام دهد ما 1دور و نزديک کردن شانه از محور بدنحرکت خواهدمیبرای مثال هنگامی که روبات
motionای که برای زير سیستم رد نمايیم . و با توجه به برنامهوا 1عدد Commandبايد در قسمت
1 Shoulder adduction/abduction
Simulink/ matlabفصل پنجم : پیاده سازی معادالت در
21
generator آمده است تغییر 213ر جدول صورت مقاديری که دزاويه های روبات به نوشته شده است
باشند.می1،1 . اين مقادير برای اين حرکت به صورت جدول نمايدمی
Shoulder adduction/abductionزوايه های مربوط به حرکت 111جدول
7θ 6θ 5θ 4θ 3θ 2θ 1θ Motion I
0 -90 0 0 0 -90 0-180 Sh abduction 1
13نیز بايد برابر منفی 6θو 2θدرجه تغییر نمايد و زاويه 113تا 3از 1θبرای انجام اين حرکت بايد زاويه
بنابراين برنامه تولید اين مقادير در زير درجه باشد و بقیه زوايا برای انجام اين حرکت صفر می باشند.
آيد.زير در می به صورت Motion Generatorسیستم
⋮
if command ==1 % Shoulder Abduction/Adduction
inc=[0 -pi/2 0 0 0 -pi/2 0]';
theta_1 = (sin(0.5*t-pi/2)+1)*pi/2 ;
ref = [theta_1 -pi/2 0 0 0 -pi/2 0]';
else if ….
⋮
.شده استاستفاده sinاز تابع 113تا 3از 1θبرای تغییر زاويه شودديده می همانطور که در برنامه فوم
گانه دست انسان نیز به صورتی که در باال اشاره شد قابل انجام شدن می باشند. 1ساير حرکات
زیر سیستم کنترلي روبات 5.2
و وظیفه يکی از آنها ورودی مرجع است ن زير سایساتم دارای ساه ورودی است کهاي 311مطابق شاکل
های موقعیت و ساارعت مفاصاال روبات دو ورودی ديگر فیدبک. بر عهده داردنوع عمل حرکتی را تعیین
Simulink/ matlabفصل پنجم : پیاده سازی معادالت در
21
زير ساایسااتم 4. برای بلوک کنترلراندخروجی مدل دينامیک روبات فیدبک گرفته شاادهکه از باشااندمی
کنترلر خطی و دينامیک معکوس ها که عبارتند از تای آندر اين جاا فقط دوتعريف شاااده اسااات کاه
ت نايز اند تا درصوربصورت ساختاری قرار گرفتهزير سیستم های کنترل نیرو و امپدانس شود.تفاده میاس
های کنترلی را به آسانی در آنها قرار داد.به ارتقای کنترلر بتوان الگوريتم
زیر سیستم کنترلي روبات 225شکل
کنترل خطي روبات 5.2.9
توان به باشد که میمی PDاساتفاده شاده در اينجا کنترلاسات. 411بلوک کنترل خطی مطابق شاکل
.ارتقا داد PIDآسانی آنرا به
Simulink/ matlabفصل پنجم : پیاده سازی معادالت در
33
روبات PDکنترلر 225شکل
روبات 9دینامیک معکوسکنترل 5.2.3
در روابط زير و استفاده uساختار کنترلر دينامیک معکوس بصورت زير است. با استفاده از قانون کنترلی
𝐾𝑃از , 𝐾𝑑 توان از صفر شدن مجانبی خطا اطمینان حاصل کرد. مثبت معین مناسب می
(13 )(q,q)q (q)M C G uq
(11 )
1 1
( , , ) (q
( ) ( )
0 ( ) ( ) ( )
0
0
,q)q (q) a
a
a
q
d D d p d
d D d p d
D p
D
q
q
p
q
q K q q K q q
M q q K q q K q q
Me K e K
u f q
e
e M
q t C G
M
M
K e M K e
1 Inverse Dynamic
Simulink/ matlabفصل پنجم : پیاده سازی معادالت در
31
که در ادامه توضیحی باشادمی 111دينامیک معکوس روبات به صاورت شاکل بلوک کنترلر مدار داخلی
شود. های آن اراهه میبرای هر يک از زير ساختار
کنترلر دینامیک معکوس روبات 525 شکل
feedback linearizerزیر سیستم 5.2.2
به منظور حدف جمتت غیر خطی تولید معادالت دينامیکی روبات رااين زير سایستم قسمت غیر خطی
,𝐶(𝑞کند )همان عبارات می �̇�)�̇� + 𝑔(𝑞) .)است. 611زير سیستم به صورت شکل ساختار داخلی اين
Simulink/ matlabفصل پنجم : پیاده سازی معادالت در
32
feedbak linearizerزیر سیستم 225شکل
tracking partزیر سیستم 5.2.2
aکنترل را انجام می دهد. يعنی trackingير سیستم در واقع کار بخش اين ز q ( را 11موجود در رابطه)
ايجاد می نمايد.
(12 )( ) ( )a d D d pq dMq K q q K q q
Simulink/ matlabفصل پنجم : پیاده سازی معادالت در
33
دینامیکي روباتزیر سیستم 5.5
زيرسیستم کنترلی و ين زير سیستم ورودی گشتاور خود را از شاود اديده می 111همانطور که در شاکل
موقعیت و دينامیک ساایسااتم گیرد.خروجیورودی مقدار اولیه خود را از زير ساایسااتم مقدار مرجع می
آمده است. 711مربوط به اين زير سیستم در شکل ساختار داخلی باشد.سرعت مفاصل روبات می
سیستم دینامیکي روبات مدار داخلي مربوط به زیر 725شکل
ت وسرع با انتیگرال گیری از آن سپس. کندتاب مفاصل روبات را تولید میش M INVتابع 711در شکل
نیز همان nonlinear part. در ضمن الزم به توضی است که زير سیستم آيدمیموقعیت مفاصل بدست
شده است.توضی داده 31411زير سیستمی است که در بخش
Simulink/ matlabفصل پنجم : پیاده سازی معادالت در
34
موقعیت و سرعت روبات سیستم زیر 5.2
های و خروجی 3بدست آمده در فصل سینماتیک مستقیماين زير سیستم با توجه به 1،1مطابق شاکل
مچ و نوک انگشتان ,آرنج ست )موقعیت و سرعت بخش های مختلف د، 1،1دينامیک سیستم در شکل
دهد. را محاسبه کرده و نتايج را نشان میدست(
رخش روباتزاویه های چزیر سیستم 5.7
های و خروجی 3اين زير سیستم با توجه به سینماتیک مستقیم بدست آمده در فصل 1،1مطابق شاکل
های مختلف دست )بازو، ساعد و ، موقعیت دورانی و سرعت دورانی بخش 1،1دينامیک سیستم در شکل
دهد. کف دست( را محاسبه کرده و نتايج را نشان می
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
31
2
ششمفصل
Simulinkشبیه سازی حرکات عمومي دست انسان در
matlab
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
36
مقدمه 2.9
مطابق اعمال گانه عمومی دساات انسااان. اين بخش اختصاااص دارد به اراهه نتايج حاصاال از حرکات نه
گانه به سایستم معرفی شده در فصل پنجم های تعريف شاده در فصال ساوم برای اين حرکات نهورودی
نتايج اين فصل حاصل شده است که در ادامه به تفصیل توضی داده خواهد شد.
9دور کردن و نزدیک کردن شانه از محور بدن شبیه سازی حرکت 2.3
باشند و همچنین در 213سطر اول جدول برای انجام اين حرکت بايد زاويه های مفاصل روبات به صورت
x,y,zموقعیت وارد شود در اين صورت 1زير سیستم مقدار دهی اولیه روبات عدد commandقسامت
آيند.در می 116مچ و نوک انگشتان ( به صورت شکل ,هريک از بخش های مختلف دست ) آرنج
Shoulder adduction/abductionمچ و نوک انگشتان ( در حرکت ,بخش های مختلف دست ) آرنج موقعیت 922شکل
1 Shoulder adduction/abduction
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
37
به صورت x,y صفحه مچ و نوک انگشتان ( در ,هريک از بخش های مختلف دسات ) آرنج x,yموقعیت
در می آيد. 216شکل
x-yدر صفحه Shoulder adduction/abductionمچ و نوک انگشتان ( در حرکت ,دست ) آرنج موقعیت 322شکل
مچ و نوک انگشتان ( در حالت سه بعدی به ,هريک از بخش های مختلف دسات ) آرنج x,y,zموقعیت
در می آيد. 316صورت شکل
در حالت سه بعدی Shoulder adduction/abductionمچ و نوک انگشتان ( در حرکت ,دست ) آرنج موقعیت 222شکل
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
31
می باشد. 416نحوه تغییر زاويه ها و گشتاورهای مفاصل به صورت شکل
Shoulder adduction/abductionنحوه تغییر زاویه ها و گشتاورهای مفاصل در حرکت 222شکل
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
31
9انبساط و انتقباض شانهشبیه سازی حرکت 2.2
باشند و همچنین در 213برای انجام اين حرکت بايد زاويه های مفاصل روبات به صورت سطر دوم جدول
x,y,zموقعیت وارد شود در اين صورت 2زير سیستم مقدار دهی اولیه روبات عدد commandقسامت
آيند.در می 116مچ و نوک انگشتان ( به صورت شکل ,هريک از بخش های مختلف دست ) آرنج
Shoulder flexion/extensionمچ و نوک انگشتان ( در حرکت ,بخش های مختلف دست ) آرنج موقعیت 522شکل
1 Shoulder flexion/extension
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
43
به صورت x,z صفحه مچ و نوک انگشتان ( در ,هريک از بخش های مختلف دسات ) آرنج x,zموقعیت
آيد.در می 616شکل
x-zدر صفحه Shoulder flexion/extensionمچ و نوک انگشتان ( در حرکت ,دست ) آرنج موقعیت 222شکل
مچ و نوک انگشتان ( در حالت سه بعدی به ,هريک از بخش های مختلف دسات ) آرنج x,y,zموقعیت
آيد.در می 716صورت شکل
در حالت سه بعدی Shoulder flexion/extensionمچ و نوک انگشتان ( در حرکت ,دست ) آرنج موقعیت 722شکل
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
41
باشد.می 116نحوه تغییر زاويه ها و گشتاورهای مفاصل به صورت شکل
Shoulder flexion/extensionنحوه تغییر زاویه ها و گشتاورهای مفاصل در حرکت 922شکل
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
42
9شبیه سازی حرکت)چرخش شانه به داخل و خارج( 2.2
باشند و همچنین 213برای انجام اين حرکت بايد زاويه های مفاصال روبات به صاورت سطر سوم جدول
وارد شااود در اين صااورت موقعیت 3زير ساایسااتم مقدار دهی اولیه روبات عدد commandدر قساامت
x,y,z آيند.در می 116مچ و نوک انگشتان ( به صورت شکل ,هريک از بخش های مختلف دست ) آرنج
Shoulder interior/exterior rotationمچ و نوک انگشتان ( در حرکت ,بخش های مختلف دست ) آرنج موقعیت 122شکل
1 Shoulder interior/exterior rotation
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
43
به صاورت شکل y,zمچ و نوک انگشاتان ( در ,هريک از بخش های مختلف دسات ) آرنج y,zموقعیت
آيد.در می 1316
x-zدر صفحه Shoulder interior/exterior rotationمچ و نوک انگشتان ( در حرکت ,دست ) آرنج موقعیت 9922شکل
مچ و نوک انگشتان ( در حالت سه بعدی به ,هريک از بخش های مختلف دسات ) آرنج x,y,zموقعیت
در می آيد. 1116صورت شکل
در حالت سه بعدی Shoulder interior/exterior rotationمچ و نوک انگشتان ( در حرکت ,دست ) آرنج موقعیت 9922شکل
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
44
باشد.می 1216نحوه تغییر زاويه ها و گشتاورهای مفاصل به صورت شکل
Shoulder interior/exterior rotationنحوه تغییر زاویه ها و گشتاورهای مفاصل در حرکت 9322شکل
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
41
9انبساط و انقباض آرنج شبیه سازی حرکت 2.5
باشند و همچنین 213برای انجام اين حرکت بايد زاويه های مفاصل روبات به صورت سطر چهارم جدول
وارد شااود در اين صااورت موقعیت 4زير ساایسااتم مقدار دهی اولیه روبات عدد commandدر قساامت
x,y,z ند.آيدر می 1316مچ و نوک انگشتان ( به صورت شکل ,هريک از بخش های مختلف دست ) آرنج
Elbow flexion/extensionمچ و نوک انگشتان ( در حرکت ,بخش های مختلف دست ) آرنج موقعیت 9222شکل
1 Elbow flexion/extension
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
46
به صورت x,z صفحهمچ و نوک انگشتان ( در ,هريک از بخش های مختلف دسات ) آرنج x, zموقعیت
آيد.در می 1416شکل
x-zدر صفحه Elbow flexion/extensionمچ و نوک انگشتان ( در حرکت ,دست ) آرنج موقعیت 9222شکل
مچ و نوک انگشتان ( در حالت سه بعدی به ,هريک از بخش های مختلف دسات ) آرنج x,y,zموقعیت
آيد.در می 1116صورت شکل
در حالت سه بعدی Elbow flexion/extensionمچ و نوک انگشتان ( در حرکت ,دست ) آرنج موقعیت 9522شکل
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
47
باشد.می 1616ورهای مفاصل به صورت شکل نحوه تغییر زاويه ها و گشتا
Elbow flexion/extensionنحوه تغییر زاویه ها و گشتاورهای مفاصل در حرکت 9222شکل
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
41
9)چرخش آرنج(شبیه سازی حرکت 2.2
باشند و همچنین 213برای انجام اين حرکت بايد زاويه های مفاصال روبات به صورت سطر پنجم جدول
وارد شااود در اين صااورت موقعیت 1زير ساایسااتم مقدار دهی اولیه روبات عدد commandدر قساامت
x,y,z د.آيندر می 1716مچ و نوک انگشتان ( به صورت شکل ,هريک از بخش های مختلف دست ) آرنج
Elbow rotationمچ و نوک انگشتان ( در حرکت ,بخش های مختلف دست ) آرنج موقعیت 9722شکل
(supination/pronation)
مچ و نوک انگشتان اابت ,ديده می شود در اين حرکت موقعیت آرنج 1716همانطور که در شکل
.باشدمی
1 Elbow rotation (supination/pronation)
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
41
به صورت شکل x,zمچ و نوک انگشااتان ( در ,هريک از بخش های مختلف دسات ) آرنج x,zموقعیت
آيد.در می 1116
x-zدر صفحه Elbow rotation (supination/pronation)مچ و نوک انگشتان ( در حرکت ,دست ) آرنج موقعیت 9922شکل
مچ و نوک انگشتان ( در حالت سه بعدی به ,هريک از بخش های مختلف دسات ) آرنج x,y,zموقعیت
آيد.در می 1116ل صورت شک
در حالت سه Elbow rotation (supination/pronation)مچ و نوک انگشتان ( در حرکت ,دست ) آرنج موقعیت 9122شکل
بعدی
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
13
باشد.می 2316نحوه تغییر زاويه ها و گشتاورهای مفاصل به صورت شکل
Elbow rotation (supination/pronation)نحوه تغییر زاویه ها و گشتاورهای مفاصل در حرکت 3922شکل
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
11
9حرکت وضعي مچبیه سازی ش ( 2.7
باشند و همچنین 213جدول ششمبرای انجام اين حرکت بايد زاويه های مفاصال روبات به صورت سطر
وارد شااود در اين صااورت موقعیت 6زير ساایسااتم مقدار دهی اولیه روبات عدد commandدر قساامت
x,y,z آيند.در می 2116مچ و نوک انگشتان ( به صورت شکل ,هريک از بخش های مختلف دست ) آرنج
Wrist ulnar/radial deviationمچ و نوک انگشتان ( در حرکت ,بخش های مختلف دست ) آرنج موقعیت 3922شکل
1 Wrist ulnar/radial deviation
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
12
به صورت y,z صفحه مچ و نوک انگشتان ( در ,هريک از بخش های مختلف دسات ) آرنج y,zموقعیت
آيد.در می 2216شکل
y-zدر صفحه Wrist ulnar/radial deviation مچ و نوک انگشتان ( در حرکت ,دست ) آرنج موقعیت 3322شکل
مچ و نوک انگشتان ( در حالت سه بعدی به ,هريک از بخش های مختلف دسات ) آرنج x,y,zموقعیت
آيد.در می 2316صورت شکل
در حالت سه بعدی Wrist ulnar/radial deviationمچ و نوک انگشتان ( در حرکت ,دست ) آرنج موقعیت 3222شکل
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
13
باشد.می 2416نحوه تغییر زاويه ها و گشتاورهای مفاصل به صورت شکل
Wrist ulnar/radial deviationنحوه تغییر زاویه ها و گشتاورهای مفاصل در حرکت 3222شکل
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
14
9انبساط و انقباض مچ( شبیه سازی حرکت 2.9
باشند و همچنین 213برای انجام اين حرکت بايد زاويه های مفاصال روبات به صورت سطر هفتم جدول
وارد شااود در اين صااورت موقعیت 7زير ساایسااتم مقدار دهی اولیه روبات عدد commandدر قساامت
x,y,z آيند.در می 2116ک انگشتان ( به صورت شکل مچ و نو ,هريک از بخش های مختلف دست ) آرنج
Wrist flexion/extensionمچ و نوک انگشتان ( در حرکت ,بخش های مختلف دست ) آرنج موقعیت 3522شکل
1 Wrist flexion/extension
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
11
به صورت x,z صفحه مچ و نوک انگشتان ( در ,هريک از بخش های مختلف دسات ) آرنج x,zموقعیت
آيد.در می 2616شکل
x-zدر صفحه Wrist flexion/extensionمچ و نوک انگشتان ( در حرکت ,دست ) آرنج موقعیت 3222شکل
مچ و نوک انگشتان ( در حالت سه بعدی به ,هريک از بخش های مختلف دسات ) آرنج x,y,zموقعیت
در می آيد. 2716صورت شکل
در حالت سه بعدی Wrist flexion/extensionمچ و نوک انگشتان ( در حرکت ,دست ) آرنج موقعیت 3722شکل
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
16
باشد.می 2116نحوه تغییر زاويه ها و گشتاورهای مفاصل به صورت شکل
Wrist flexion/extension مفاصل در حرکت یها و گشتاورها هیزاو ریینحوه تغ 3922شکل
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
17
9انبساط و انقباض افقي شانهشبیه سازی حرکت 2.1
باشند و همچنین 213برای انجام اين حرکت بايد زاويه های مفاصل روبات به صورت سطر هشتم جدول
وارد شااود در اين صااورت موقعیت 1زير ساایسااتم مقدار دهی اولیه روبات عدد commandدر قساامت
x,y,z د.آيندر می 2116مچ و نوک انگشتان ( به صورت شکل ,هريک از بخش های مختلف دست ) آرنج
Wrist flexion/extensionمچ و نوک انگشتان ( در حرکت ,بخش های مختلف دست ) آرنج موقعیت 3122شکل
1 Shoulder horizontal flexion/extension
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
11
به صورت x,z صفحه مچ و نوک انگشتان ( در ,هريک از بخش های مختلف دسات ) آرنج y,zموقعیت
آيد.در می 3316شکل
y-zدر صفحه Shoulder horizontal flexion/extensionدر حرکت مچ و نوک انگشتان ( ,دست ) آرنج موقعیت 2922شکل
مچ و نوک انگشتان ( در حالت سه بعدی به ,هريک از بخش های مختلف دسات ) آرنج x,y,zموقعیت
در می آيد. 3116صورت شکل
در حالت سه Shoulder horizontal flexion/extensionمچ و نوک انگشتان ( در حرکت ,دست ) آرنج موقعیت 2922شکل
بعدی
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
11
باشد.می 3216نحوه تغییر زاويه ها و گشتاورهای مفاصل به صورت شکل
Shoulder horizontal flexion/extension مفاصل در حرکت یها و گشتاورها هیزاو ریینحوه تغ 2322شکل
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
63
9چرخش کامل شانهحرکت سازی شبیه 2.99
باشند و همچنین در 213برای انجام اين حرکت بايد زاويه های مفاصل روبات به صورت سطر نهم جدول
x,y,zوارد شود در اين صورت موقعیت 1زير سیستم مقدار دهی اولیه روبات عدد commandقسامت
.آينددر می 3316مچ و نوک انگشتان ( به صورت شکل ,هريک از بخش های مختلف دست ) آرنج
Full-range shoulder free motionمچ و نوک انگشتان ( در حرکت ,بخش های مختلف دست ) آرنج موقعیت 2222شکل
1 Full-range shoulder free motion
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
61
به صورت y,z صفحه مچ و نوک انگشاتان ( در ,هريک از بخش های مختلف دسات ) آرنج y,zموقعیت
آيد.در می 3416شکل
x-zدر صفحه Full-range shoulder free motionمچ و نوک انگشتان ( در حرکت ,دست ) آرنج موقعیت 2222شکل
مچ و نوک انگشتان ( در حالت سه بعدی به ,هريک از بخش های مختلف دسات ) آرنج x,y,zموقعیت
در می آيد. 3116صورت شکل
در حالت سه بعدی Full-range shoulder free motionمچ و نوک انگشتان ( در حرکت ,دست ) آرنج موقعیت 2522شکل
Simulink/ matlabفصل ششم : شبیه سازی حرکات عمومی دست انسان در
62
باشد.می 3616نحوه تغییر زاويه ها و گشتاورهای مفاصل به صورت شکل
Full-range shoulder free motion مفاصل در حرکت یها و گشتاورها هیزاو ریینحوه تغ 2222شکل
فصل هفتم: مراحل ساخت انیمیشن روبات
63
7
فصل هفتم
ساخت انیمیشن ربات
فصل هفتم: مراحل ساخت انیمیشن روبات
64
ربات شنیمیمراحل ساخت ان 7.9
مشاهده 117 مايا که نمای کلی آن را در شکل افزاردر اين بخش ابتدا به تشري نحوه طراحی ربات در نرم
پیوند مدل در متلب و مکانیک طراحی شده و انیمیشن و نهايتا به تشري ارتباط شده کنید، پرداختهمی
پردازيم.نهايی می
افزار مایانمای نرم 927شکل
لینک به ترتیب با سه، يک و سه درجه آزادی را وارد محیط نموده و 3ابتدا 317و 217های مطابق شکل
دهیم.قرار میها مدل را تشکیل داده و مطابق مدل اصلی آن
اول( نکی)وارد کردن ل امای افزار نرم ینما 327شکل
فصل هفتم: مراحل ساخت انیمیشن روبات
61
)وارد کردن مفصل اول( امای افزار¬نرم ینما 227شکل
باشد. ما به دنبال اين هستیم که ها همگی قابل تنظیم می طول و اندازه لینک 317و 217که مطابق شکل
با آن حرکت کنند و به همین ترتیب با حرکت دستگاه های دو تا هفت با حرکت دستگاه اول همه دستگاه
ا ها به هم ر ست وضعیت لینکيهای سه تا هفت با آن حرکت کنند برای اين کار می بادوم همه دستگاه
مطابق شکل مشخص نمايیم.
واض است که اگر هر لینک را فرزند لینک قبلی خود بگیريم اين فرزند حرکت والدين خود را تعقیب
کردن groupو همچنین 417مطابق شکل children و parentپس با بکارگیری دو متد کندمی
نمايیم. الزم به ذکر است ها را معلوم می وضعیت اين لینک 117های با سه درجه آزادی مطابق شکل لینک
گزينیم که کلیک کرده و سپس فرزندان آن را برمی parentابتدا روی لینک parentبرای بکارگیری متد
دهد. همچنین ها را نشان می رسیم که رابطه لینکمی 117پس از انجام اين کار به لیستی مطابق شکل
groupدرجه آزادی خودش را با خودش 3بار به دلیل داشتن 3کردن يک لینک با خودش groupبرای
کنیم.می
فصل هفتم: مراحل ساخت انیمیشن روبات
66
کردن( parent) امای افزار¬نرم ینما 227شکل
کردن( group) امای افزار¬نرم ینما 527شکل
(ها¬نکیل تی)وضع امای افزار¬نرم ینما 227شکل
فصل هفتم: مراحل ساخت انیمیشن روبات
67
ذخیره کرده تا بتوانیم در متلب باز wrlکنید را با فرمت مشاهده می717نهايتا مدل نهايی که در شکل
.نموده تا انیمیشن را بسازيم
شده يربات طراح یينها ینما 727شکل
ها را به هم ، وضاااعیت لینک117مطابق شاااکل editدر نرم افزار متلب مدل را وارد کرده و در قسااامت
بینیم.می
را در متلب ها نکیل تیوضع شینما 927شکل
فصل هفتم: مراحل ساخت انیمیشن روبات
61
کنیم که ورودی مورد نظر زوايای چرخش مفاصل است. و با اين تنظیم هفت در اينجا تعیین می
ها د، اين ورودینگیر شود که به ترتیب هريک چهار ورودی میروی بلوک ظاهر می117ورودی مطابق شکل
ای که تعیین کننده محور چرخش است و يک اسکالر که زاويه چرخش به مولفه 3عبارتند از يک بردار
ورت صبات در حالت ابتدايی دقیقا به شکل دست انسان باشد يکسری زاويه بهوراديان است. برای اين که ر
م.دهیست نیز میآف
بمتل در ها بلوک شینما 127شکل
بريم. می بهره VR sinkبرای تطبیق و دادن اين چهار ورودی به بلوک signal expanderاز بلوک
مشاهده 111ای مطابق شکلبات را در پنجرهوها حرکت رکردن متلب و تغییر ورودی Runنهايتا با
نمايیم.می
فصل هفتم: مراحل ساخت انیمیشن روبات
61
در متلب شنیمیان شینما 9927شکل
فصل هشتم : نتیجه گیری
73
9
فصل هشتم
نتیجه گیری
فصل هشتم : نتیجه گیری
71
درجه آزادی مورد بررسی قرار 7سازی حرکات عمومی دست انسان توسط يک روبات در اين پروژه شبیه
ت شود و قرار اسگرفت. روبات مورد بررسی عبارتست از يک اسکلت خارجی که توسط انسان پوشیده می
کی از اهداف تمام حرکات دست انسان را بطور کامل و بدون هیچ عیب و نقصی انجام دهد. از همین رو ي
اين امر 3و 2اساسی اين پروژه شناسايی حرکات عمومی دست انسان با جزويات تمام است که در فصل
بايست حرکاتی انجام شاده اسات. از آنجايی که روبات در تعامل همیشاگی با دسات انسان قرار دارد می
معادالت دينامیکی دست انسان مشابه حرکات انسان داشته باشد و از اينرو معادالت دينامیکی آن مشابه
نحوه 1اسااتخراج شااد و در فصاال 4اساات. معادالت دينامیکی دساات انسااان و بازوی روباتی در فصاال
درجه آزادی پیشنهادی به 7نشان دادند که روبات 6سازی آن شرح داده شد. نتايج حاصل از فصل شبیه
ت را بخوبی دارد. همچنین عنوان اساااکلت خارجی دسااات انساااان قابلیت انجام حرکات عمومی دسااا
های اين فصال حاکی از عبور روبات از نقاط تکینگی )يا خیلی نزديک به تکینگی( است که ساازیشابیه
هايی مانند آفساات دادن به زوايای مفاصاال برای قرار دادن نقطه بايساات به ترفندبرای پرهیز از آنها می
. تکینگی در خارج از محدوده حرکتی دست انسان متوسل شد
مراجع
72
مراجعمنابع و
[1]
Joel C. Perry, Jacob Rosen, and Stephen Burns; “Upper-Limb Powered Exoskeleton Design”,
IEEE/ASME TRANSACTIONS ON MECHATRONICS, VOL. 12, NO. 4, AUGUST 2007
[2]
Joel C. Perry, Jacob Rosen, and Stephen Burns; “The Human Arm Kinematics and Dynamics During Daily Activities – Toward a 7 DOF Upper Limb Powered Exoskeleton”, Advanced
Robotics, 2005. ICAR '05. Proceedings., 12th International Conference on 18-20 July 2005 page
532 - 539
[3]
Paolo de Leva; “Adjustments to Zatsiorsky-Seluyanov's segment inertia parameters”, Journal of
Defining Arm lenghts [ref: zatsiorsky]U__L := 0.245; # male upper arm length , for female use 0.236F__L := 0.251; # male forearm length , for female use 0.247H__L := 0.190; # male hand lenght , for female use 0.172
Transformation Matricies# Transformation Matrix from DH ParametersTrans d proc alpha, a, d, theta Matrix cos theta , Ksin theta , 0 , a , sin theta * cos alpha , cos theta * cos alpha , Ksin alpha , Ksin alpha * d , sin theta * sin alpha , cos theta * sin alpha , cos alpha , cos alpha * d , 0 , 0 , 0 , 1 ;end proc:Defining Homogenious TransformationsT__1 := (Trans(0,0,0,theta__1));T__2 := (Trans(Pi/2,0,0,theta__2));T__3 := (Trans(-Pi/2,0,-U__L,theta__3));T__4 := (Trans(Pi/2,0,0,theta__4));T__5 := (Trans(-Pi/2,0,-F__L,theta__5));T__6 := (Trans(Pi/2,0,0,theta__6));T__7 := (Trans(Pi/2,0,0,theta__7));T__E := (Trans(0,H__L,0,0));
# Defining Rotation Matrix From TransformationR1 d T1 1 ..3, 1 ..3 :R2 d T2 1 ..3, 1 ..3 :R3 d T3 1 ..3, 1 ..3 :R4 d T4 1 ..3, 1 ..3 :R5 d T5 1 ..3, 1 ..3 :R6 d T6 1 ..3, 1 ..3 :R7 d T7 1 ..3, 1 ..3 :
Generating Elbow position, Wrist position and End of Middle finger position with respect to frame attached at Body in shoulder Joint.
P__E := (T__1 . T__2. T__3 . Matrix([[0],[0],[0],[1]]))[1..3]; # Positionof ElbowP__W := (T__1 . T__2. T__3 . T__4 . T__5 . Matrix([[0],[0],[0],[1]]))[1..3]: # Position of WristP__H := (T__1 . T__2. T__3 . T__4 . T__5 . T__6 . T__7 . T__E)[1..3,4]: #Position of middle Finger last point
Executing Linear Jacobian
> >
> > > >
LJacobian d proc position LJ d Matrix 3, 7 : LJ 1 ..3, 1 d map diff, position, theta1 : LJ 1 ..3, 2 d map diff, position, theta2 : LJ 1 ..3, 3 d map diff, position, theta3 : LJ 1 ..3, 4 d map diff, position, theta4 : LJ 1 ..3, 5 d map diff, position, theta5 : LJ 1 ..3, 6 d map diff, position, theta6 : LJ 1 ..3, 7 d map diff, position, theta7 : LJ end proc:Warning, `LJ` is implicitly declared local to procedure `LJacobian`
Jv__E := LJacobian(P__E): # Linear Jacobian of Elbow Jv__W := LJacobian(P__W): # Linear Jacobian of WristJv__Fe := LJacobian(P__H): # Linear Jacobian of finger-end point
Angular Velocity Jacobian# Calculating Angular Velocities of Links in base and link frameKd d Matrix 0 , 0 , 1 ;## angular velocity of Upper ArmOmegaU d dt1 * Kd C dt2 * R1 . R2 . Kd C dt3 * R1 . R2 . R3 .Kd : # In base frameomegaU d simplify Transpose R1 . R2 . R3 .OmegaU : # in Link Frameomegau d dt3 * Kd C dt2 * Transpose R3 . Kd Cdt1 * Transpose R2.R3 . Kd; # In link Framesimplify omegauKomegaU : # Check of Correction ## angular velocity of Fore ArmOmegaF d simplify OmegaU C dt4 * R1 . R2 . R3 . R4 . Kd C dt5 * R1 . R2 . R3 . R4 . R5 . Kd : # In base frameomegaF d simplify Transpose R1 . R2 . R3 . R4 . R5 .OmegaF :
# In Link frameomegaf d simplify dt5 * Kd C dt4 * Transpose R5 . Kd C dt3 * Transpose R4.R5 . Kd C dt2 * Transpose R3.R4.R5 . Kd C dt1 * Transpose R2.R3.R4.R5 . Kd :
# In Link framesimplify omegaF K omegaf :
# Check of Correction ## angular velocity of HandOmegaH d simplify OmegaF C dt6 * R1 . R2 . R3 . R4 . R5 . R6 .Kd C dt7 * R1 . R2 . R3 . R4 . R5 . R6 . R7 .Kd :
# In base frameomegaH d simplify Transpose R1 . R2 . R3 . R4 . R5 . R6 . R7 .OmegaH : # In Link frameomegah d simplify dt7 * Kd C dt6 * Transpose R7 . Kd C dt5 * Transpose R6.R7 . Kd C dt4 * Transpose R5.R6.R7 . Kd C dt3 * Transpose R4.R5.R6.R7 . Kd C dt2 * Transpose R3.R4.R5.R6.R7 . Kd C dt1 * Transpose R2.R3.R4.R5.R6.R7 . Kd : # In Link framesimplify omegaH K omegah :
# Check of Correction
(5.1)(5.1)
(5.1)(5.1)
0
0
1
dt2 Transpose T31 ..3, 1 ..3.
0
0
1
C dt1 Transpose T21 ..3, 1 ..3.T31 ..3, 1 ..3
.
0
0
1
C
0
0
dt3
Calculation of Angular Velocity Jacobianexpr1 d Vector omegau 1 ..3, 1 :expr2 d Vector omegaf 1 ..3, 1 :expr3 d Vector omegah 1 ..3, 1 :with VectorCalculus :dt d dt1, dt2, dt3, dt4, dt5, dt6, dt7 :jou d Jacobian expr1, dt ; # Angular Jacobian of Upper Armjof d Jacobian expr2, dt ; # Angular Jacobian of Fore Armjoh d Jacobian expr3, dt : # Angular Jacobian of Hand
Center of masses, their Positions and moment of inertias mU d 2; # mass of Upper armPU d 0.57 * PE : # Position of CM of upper armIU d Matrix 3, 3 :IU 3, 3 d mU * 0.158 * UL ^2 : IU 1, 1 d mU * 0.285 * UL ^2 :IU 2, 2 d mU * 0.269 * UL ^2 : mF d 1.18; # mass of ForearmPF d 0.46 * PW : # Position of CM of Fore armIF d Matrix 3, 3 :IF 3, 3 d mF * 0.121 * FL ^2 : # IzzU r
fz^2:#IF 1, 1 d mF * 0.276 * FL ^2 : # IxxU r
fx^2:#IF 2, 2 d mF * 0.265 * FL ^2 : # IyyU r
fy^2:##`#` mH d 0.445; # mass of HandPcH d 0.79 * PH : # Position of CM of HandIH d Matrix 3, 3 :IH 3, 3 d mH * 0.628 * HL ^2 : IH 1, 1 d mH * 0.401 * HL ^2 : IH 2, 2 d mH * 0.513 * HL ^2 : IU d IU;IF d IF;IH d IH;
> >
> > > >
Linear Jacobian of Center of massesJv__U := LJacobian(P__U):# Linear Jacobian of Upper Arm Center of massJv__F := LJacobian(P__F): # Linear Jacobian of Forearm Center of massJv__H := LJacobian(P__cH): # Linear Jacobian of Hand Center of mass
Inertia Tensor, Kinetic Energy and potential Energy Inertia d mU * Transpose JvU . JvU C mF * Transpose JvF .JvF C mH * Transpose JvH . JvH C Transpose jou . IU . jou C Transpose jof . IF . jof C Transpose joh . IH . joh :
# Executing Potential Energy of the systemUU d mU * 9.81 * PU 1, 1 : #Potential Energy of Upper armUF d mF * 9.81 * PF 1, 1 : #Potential Energy of Fore armUH d mH * 9.81 * PcH 1 : #Potential Energy of HandU d UU C UF C UH ; # Huamn Arm Potential Energy
LAGRANGIAN AND EQUATIONS OF MOTIONS# Compose Lagrangian and equatiom of motion theta1 d phi1 t ; theta2 d phi2 t : theta3 d phi3 t :theta4 d phi4 t : theta5 d phi5 t : theta6 d phi6 t :theta7 d phi7 t :dphi d Matrix diff phi1 t , t , diff phi2 t , t , diff phi3 t , t , diff phi4 t , t , diff phi5 t , t , diff phi6 t , t , diff phi7 t , t :KineticE d 0.5 * Transpose dphi .Inertia.dphi 1, 1 :PotentiE d U : Lag d KineticE K PotentiE :with VariationalCalculus :EQ d EulerLagrange KLag, t, phi1 t , phi2 t , phi3 t , phi4 t , phi5 t , phi6 t , phi7 t :
simplification and chenge variables in order to transfer to MATLABfor i from 1 by 1 to 7 do eq__ i d simplify EQ i :eq__ i d algsubs diff phi1 t , t, t = ddphi
1, eq__ i :
eq__ i d algsubs diff phi2 t , t, t = ddphi2, eq__ i :
eq__ i d algsubs diff phi3 t , t, t = ddphi3, eq__ i :
eq__ i d algsubs diff phi4 t , t, t = ddphi4, eq__ i :
eq__ i d algsubs diff phi5 t , t, t = ddphi5, eq__ i :
eq__ i d algsubs diff phi6 t , t, t = ddphi6, eq__ i :
eq__ i d algsubs diff phi7 t , t, t = ddphi7, eq__ i :
eq__ i d algsubs diff phi1 t , t = dphi1, eq__ i :
eq__ i d algsubs diff phi2 t , t = dphi2, eq__ i :
eq__ i d algsubs diff phi3 t , t = dphi3, eq__ i :
eq__ i d algsubs diff phi4 t , t = dphi4, eq__ i :
eq__ i d algsubs diff phi5 t , t = dphi5, eq__ i :
eq__ i d algsubs diff phi6 t , t = dphi6, eq__ i :
eq__ i d algsubs diff phi7 t , t = dphi7, eq__ i :
eq__ i d algsubs cos phi1 t = C1, eq__ i :eq__ i d algsubs cos phi2 t = C2, eq__ i :eq__ i d algsubs cos phi3 t = C3, eq__ i :eq__ i d algsubs cos phi4 t = C4, eq__ i :eq__ i d algsubs cos phi5 t = C5, eq__ i :eq__ i d algsubs cos phi6 t = C6, eq__ i :eq__ i d algsubs cos phi7 t = C7, eq__ i :eq__ i d algsubs sin phi1 t = S1, eq__ i :eq__ i d algsubs sin phi2 t = S2, eq__ i :eq__ i d algsubs sin phi3 t = S3, eq__ i :eq__ i d algsubs sin phi4 t = S4, eq__ i :eq__ i d algsubs sin phi5 t = S5, eq__ i :eq__ i d algsubs sin phi6 t = S6, eq__ i :eq__ i d algsubs sin phi7 t = S7, eq__ i :end do :# correct number of equationseq1 d eq__ 6 :eq2 d eq__ 7 :eq3 d eq__ 4 :eq4 d eq__ 5 :eq5 d eq__ 3 :eq6 d eq__ 1 :eq7 d eq__ 2 :#eq__ 1 d eq1 :eq__ 2 d eq2 :eq__ 3 d eq3 :eq__ 4 d eq4 :eq__ 5 d eq5 :eq__ 6 d eq6 :
eq__ 7 d eq7 :
INERTIA TENSOR# Inertia Tensor IM d Matrix diff eq__ 1 , ddphi
1 , diff eq__ 1 , ddphi
2 , diff eq__ 1 , ddphi
3 , diff eq__ 1 ,
ddphi4
, diff eq__ 1 , ddphi5
, diff eq__ 1 , ddphi6
, diff eq__ 1 , ddphi7
,
diff eq__ 2 , ddphi1
, diff eq__ 2 , ddphi2
, diff eq__ 2 , ddphi3
, diff eq__ 2 , ddphi4
,
diff eq__ 2 , ddphi5
, diff eq__ 2 , ddphi6
, diff eq__ 2 , ddphi7
,
diff eq__ 3 , ddphi1
, diff eq__ 3 , ddphi2
, diff eq__ 3 , ddphi3
, diff eq__ 3 , ddphi4
,
diff eq__ 3 , ddphi5
, diff eq__ 3 , ddphi6
, diff eq__ 3 , ddphi7
,
diff eq__ 4 , ddphi1
, diff eq__ 4 , ddphi2
, diff eq__ 4 , ddphi3
, diff eq__ 4 , ddphi4
,
diff eq__ 4 , ddphi5
, diff eq__ 4 , ddphi6
, diff eq__ 4 , ddphi7
,
diff eq__ 5 , ddphi1
, diff eq__ 5 , ddphi2
, diff eq__ 5 , ddphi3
, diff eq__ 5 , ddphi4
,
diff eq__ 5 , ddphi5
, diff eq__ 5 , ddphi6
, diff eq__ 5 , ddphi7
,
diff eq__ 6 , ddphi1
, diff eq__ 6 , ddphi2
, diff eq__ 6 , ddphi3
, diff eq__ 6 , ddphi4
,
diff eq__ 6 , ddphi5
, diff eq__ 6 , ddphi6
, diff eq__ 6 , ddphi7
,
diff eq__ 7 , ddphi1
, diff eq__ 7 , ddphi2
, diff eq__ 7 , ddphi3
, diff eq__ 7 , ddphi4
,
diff eq__ 7 , ddphi5
, diff eq__ 7 , ddphi6
, diff eq__ 7 , ddphi7
:
NONLINEAR PARTS# nonlinear/graviational part exploited when acceeration is zero (ddq =0); H1 d simplify subs ddphi
1= 0, ddphi
2= 0, ddphi
3= 0, ddphi
4= 0, ddphi
5= 0, ddphi
6= 0, ddphi
7= 0, eq__ 1 :
H2 d simplify subs ddphi1
= 0, ddphi2
= 0, ddphi3
= 0, ddphi4
= 0, ddphi5
= 0, ddphi6
= 0, ddphi7
= 0, eq__ 2 :
H3 d simplify subs ddphi1
= 0, ddphi2
= 0, ddphi3
= 0, ddphi4
= 0, ddphi5
= 0, ddphi6
= 0, ddphi7
= 0, eq__ 3 :
H4 d simplify subs ddphi1
= 0, ddphi2
= 0, ddphi3
= 0, ddphi4
= 0, ddphi5
= 0, ddphi6
= 0, ddphi7
= 0, eq__ 4 :
H5 d simplify subs ddphi1
= 0, ddphi2
= 0, ddphi3
= 0, ddphi4
= 0, ddphi5
= 0, ddphi6
= 0, ddphi7
= 0, eq__ 5 :
H6 d simplify subs ddphi1
= 0, ddphi2
= 0, ddphi3
= 0, ddphi4
= 0, ddphi5
= 0, ddphi6
= 0, ddphi7
= 0, eq__ 6 :
H7 d simplify subs ddphi1
= 0, ddphi2
= 0, ddphi3
= 0, ddphi4
= 0, ddphi5
= 0, ddphi6
= 0, ddphi7
= 0, eq__ 7 :
Inertia Tensr Component to MatlabIM = map simplified, IM : #Matlab(IM[1,1],resultname="M11"); Matlab(IM[1,2],resultname="M12"); #Matlab(IM[1,3],resultname="M13"); Matlab(IM[1,4],resultname="M14"); #Matlab(IM[1,5],resultname="M15"); Matlab(IM[1,6],resultname="M16"); #Matlab(IM[1,7],resultname="M17"); Matlab(IM[2,2],resultname="M22"); #Matlab(IM[2,3],resultname="M23"); Matlab(IM[2,4],resultname="M24");
Nonlinear Part to matlab#Matlab(H__1,resultname="h1");#Matlab(H__2,resultname="h2");#Matlab(H__3,resultname="h3");#Matlab(H__4,resultname="h4");#Matlab(H__5,resultname="h5");#Matlab(H__6,resultname="h6");#Matlab(H__7,resultname="h7");