東京工業大学工学院機械系 教授
イワツキ ノブユキ
岩附 信行
大学院講義
「先端機械要素」
Tokyo Institute of TechnologyDept of Mechanical EngineeringSchool of Engineering
Prof Nobuyuki Iwatsuki
Lecture for Graduate Students
Advanced Mechanical Elements
Lecture syllabus
Course title Advanced Mechanical ElementsAcademic major Mechanical EngineeringOffered quarter 2QDayperiod Thursday 3-4Credits 1-0-0Lecture room ZOOMRegistration number MECHH431Lecturer Prof Nobuyuki Iwatsuki
Office I1-305E-mail iwatsukinaamtitechacjp
Course description and aimsThis course offers the knowledge on kinematic and dynamic
analysis of planar and spatial link mechanisms and methods to control redundant and overunderactuated mechanisms
The characteristic of motion mechanisms affects the performance of advanced mechanical systems such as robots It is thus required to quantitatively evaluate the characteristic and to design and control mechanisms based on the evaluation criteria
For this purpose students will understand how to analyze the kinematics and dynamics of planar and spatial closed-loop link mechanisms by utilizing the systematic kinematic analysis method and will experience to apply them to kinematic and dynamic analyses and motion control of redundant mechanisms overactuated mechanisms of which actuator inputs are more than mobility of mechanism and underactuated mechanisms constrained by elastic elements or gravitational force
Student learning outcomesBy the end of this course students will be able to
(1)Explain mobility of mechanism and relation between inputoutput motion of mechanism
(2)Analyze displacement velocity and acceleration of planarspatialclosed-loop link mechanisms with the systematic kinematic analysis method
(3)Analyze the dynamics of planarspatial closed-loop link mechanisms utilizing the systematic kinematic analysis method
(4)Explain the optimum motion control of redundant link mechanisms
(5)Explain motion control of overactuated or underactuatedmechanisms with elastic elements
KeywordsClose-loop link mechanism kinematics dynamics systematic kinematic analysis motion control optimum control redundancy overactuated mechanism underactuated mechanism wire-driven mechanism
Class flowImportant issues are summarized at the end of lecture every week Because sample software to analyze mechanisms which are explained in this lecture will be offered to students students are expected to try to analyze various mechanisms with the software by themselves so as to understand what they learn
ScheduleClass 1 Kinematic analysis of planar link mechanism with the (June 25) systematic kinematic analysis method
- Displacement velocity and acceleration analyses of planar closed-loop link mechanism with the systematic kinematic analysis method -
Class 2 Kinematic analysis of spatial link mechanism with the(July 2) systematic kinematic analysis method
- Expansion of the systematic kinematic analysis method to spatial mechanisms -
Class 3 Dynamic analysis of planarspatial link mechanisms (July 9) - Driving force and joint force analysis using the
systematic kinematic analysis -Class 4 Motion control of redundant link mechanisms(July 16) - Optimum motion control to utilize redundancy -
Class 5 Kinematic analysis and motion control of overactuated(July 23) mechanisms
- Motion control of overactuated mechanisms using relaxation with elastic elements -
Class 6 Kinematic analysis and motion control of underactuated(July 30) mechanisms with elastic elements
- Motion control of underactuated mechanisms constrained by elastic elements -
Class 7 Kinematic analysis and motion control of underactuated(August 6) wire-driven mechanisms
- Motion control of wire-driven underactuatedmechanisms under gravitational force-
Textbook reference book course materialNo textbook is required Several handouts will be distributedvia OCW-i Students should download and print the handouts before class The following book is recommended as a reference book
JSME Textbook series Kinematics of Machineryedited by the Japan Society of Mechanical Engineers (2007)
Assessment criteria and methodsStudents knowledge of kinematic and dynamic analyses of planarspatial closed-loop link mechanisms with the systematic kinematic analysis method and their application to analyses and motion controls of redundant and underoveractuated mechanisms is assessed via submitted reports on several issues
Related coursesSynthesis of robotic mechanisms Design of robot controller
PrerequisitesStudents must have successfully completed Mechanical elements and draftings or have equivalent knowledge
June 25 2020
Advanced Mechanical Elements(Lecture 1)
Kinematic analysis of planar link mechanism with the systematic kinematic analysis method
Tokyo Institute of TechnologyDept of Mechanical EngineeringSchool of Engineering
Prof Nobuyuki Iwatsuki
1 Introduction of lecturer
Please visit my WEB-sitehttpwwwdynamicsmeptitechacjpindexphp
orSearch by Google with a keyword lsquoIwatsuki labrsquo
Nobuyuki Iwatsuki Professor Dr EngAffiliation
Dynamics Engineering LaboratoryDepartment of Mechanical EngineeringSchool of EngineeringTokyo Institute of Technology
OfficeRoom 305 Ishikawadai-1 Building Ohokayama Campus
Contact Tel +81-3-5734-2538Fax +81-3-5734-3917 (Department office)E-mail nobmeptitechacjp
Several artistic works
Research Themes(A)Synthesis and Control of Robotic Mechanisms
Especially focused on lsquoMechanism Design and Motion Control of Hyper RedundantUnderactuatedRobotsrsquo
(B)Silent EngineeringEspecially focused on lsquoEstimation of Sound Power Radiating from Vibrating Structure and Structural Optimization to Reduce the Sound Powerrsquo
(C)Functional Material ActuatorsEspecially focused on lsquoDevelopment of Micro CiliaryActuators in Grouprsquo
Several Examples of My Research Works(1)Biped Walking Machine
ndash While undergraduatePhD student (-1987)
Straight Walk of Biped Walking Machine(1982)
Knee joint
Ankle joint
Toe
Heel
Hip joint
Leg Mechanism with 1 DOF
Ankle path generator(Planar 6-bar link mech)
Foot driving mechanism(Planar 8-bar link mech)
Driving links
Planar 8-bar link mechanism
Counter weightDrawing 8 figure
Plane-cylinder joint
Slanting crank shaft
Attitude Stabilizing Mechanism
950m
m
A Biped Waking Machine with only 1 DOF (Mechanical synthesis for simple control)
Walking speed 120 stepsmin (World record)
2nd prototype which can startand stop walking
Improvement for non steady walkingChange strideIndependent drive of foot driving mechanismArbitrary control of attitude stabilizing mechanismA new prototype with 5 DOF
Items Prototype-1 Prototype-2Degree-of-freedom 1 5Weight 206kgf 488kgfStraight walk Max 120stepmin Max15 stepsminStartstop to walk - Sucessful
Dynamics is important not only to stabilize biped machine but also to drive actuators
Tokyo Institute of TechnologyDept of Mechanical EngineeringSchool of Engineering
Prof Nobuyuki Iwatsuki
Lecture for Graduate Students
Advanced Mechanical Elements
Lecture syllabus
Course title Advanced Mechanical ElementsAcademic major Mechanical EngineeringOffered quarter 2QDayperiod Thursday 3-4Credits 1-0-0Lecture room ZOOMRegistration number MECHH431Lecturer Prof Nobuyuki Iwatsuki
Office I1-305E-mail iwatsukinaamtitechacjp
Course description and aimsThis course offers the knowledge on kinematic and dynamic
analysis of planar and spatial link mechanisms and methods to control redundant and overunderactuated mechanisms
The characteristic of motion mechanisms affects the performance of advanced mechanical systems such as robots It is thus required to quantitatively evaluate the characteristic and to design and control mechanisms based on the evaluation criteria
For this purpose students will understand how to analyze the kinematics and dynamics of planar and spatial closed-loop link mechanisms by utilizing the systematic kinematic analysis method and will experience to apply them to kinematic and dynamic analyses and motion control of redundant mechanisms overactuated mechanisms of which actuator inputs are more than mobility of mechanism and underactuated mechanisms constrained by elastic elements or gravitational force
Student learning outcomesBy the end of this course students will be able to
(1)Explain mobility of mechanism and relation between inputoutput motion of mechanism
(2)Analyze displacement velocity and acceleration of planarspatialclosed-loop link mechanisms with the systematic kinematic analysis method
(3)Analyze the dynamics of planarspatial closed-loop link mechanisms utilizing the systematic kinematic analysis method
(4)Explain the optimum motion control of redundant link mechanisms
(5)Explain motion control of overactuated or underactuatedmechanisms with elastic elements
KeywordsClose-loop link mechanism kinematics dynamics systematic kinematic analysis motion control optimum control redundancy overactuated mechanism underactuated mechanism wire-driven mechanism
Class flowImportant issues are summarized at the end of lecture every week Because sample software to analyze mechanisms which are explained in this lecture will be offered to students students are expected to try to analyze various mechanisms with the software by themselves so as to understand what they learn
ScheduleClass 1 Kinematic analysis of planar link mechanism with the (June 25) systematic kinematic analysis method
- Displacement velocity and acceleration analyses of planar closed-loop link mechanism with the systematic kinematic analysis method -
Class 2 Kinematic analysis of spatial link mechanism with the(July 2) systematic kinematic analysis method
- Expansion of the systematic kinematic analysis method to spatial mechanisms -
Class 3 Dynamic analysis of planarspatial link mechanisms (July 9) - Driving force and joint force analysis using the
systematic kinematic analysis -Class 4 Motion control of redundant link mechanisms(July 16) - Optimum motion control to utilize redundancy -
Class 5 Kinematic analysis and motion control of overactuated(July 23) mechanisms
- Motion control of overactuated mechanisms using relaxation with elastic elements -
Class 6 Kinematic analysis and motion control of underactuated(July 30) mechanisms with elastic elements
- Motion control of underactuated mechanisms constrained by elastic elements -
Class 7 Kinematic analysis and motion control of underactuated(August 6) wire-driven mechanisms
- Motion control of wire-driven underactuatedmechanisms under gravitational force-
Textbook reference book course materialNo textbook is required Several handouts will be distributedvia OCW-i Students should download and print the handouts before class The following book is recommended as a reference book
JSME Textbook series Kinematics of Machineryedited by the Japan Society of Mechanical Engineers (2007)
Assessment criteria and methodsStudents knowledge of kinematic and dynamic analyses of planarspatial closed-loop link mechanisms with the systematic kinematic analysis method and their application to analyses and motion controls of redundant and underoveractuated mechanisms is assessed via submitted reports on several issues
Related coursesSynthesis of robotic mechanisms Design of robot controller
PrerequisitesStudents must have successfully completed Mechanical elements and draftings or have equivalent knowledge
June 25 2020
Advanced Mechanical Elements(Lecture 1)
Kinematic analysis of planar link mechanism with the systematic kinematic analysis method
Tokyo Institute of TechnologyDept of Mechanical EngineeringSchool of Engineering
Prof Nobuyuki Iwatsuki
1 Introduction of lecturer
Please visit my WEB-sitehttpwwwdynamicsmeptitechacjpindexphp
orSearch by Google with a keyword lsquoIwatsuki labrsquo
Nobuyuki Iwatsuki Professor Dr EngAffiliation
Dynamics Engineering LaboratoryDepartment of Mechanical EngineeringSchool of EngineeringTokyo Institute of Technology
OfficeRoom 305 Ishikawadai-1 Building Ohokayama Campus
Contact Tel +81-3-5734-2538Fax +81-3-5734-3917 (Department office)E-mail nobmeptitechacjp
Several artistic works
Research Themes(A)Synthesis and Control of Robotic Mechanisms
Especially focused on lsquoMechanism Design and Motion Control of Hyper RedundantUnderactuatedRobotsrsquo
(B)Silent EngineeringEspecially focused on lsquoEstimation of Sound Power Radiating from Vibrating Structure and Structural Optimization to Reduce the Sound Powerrsquo
(C)Functional Material ActuatorsEspecially focused on lsquoDevelopment of Micro CiliaryActuators in Grouprsquo
Several Examples of My Research Works(1)Biped Walking Machine
ndash While undergraduatePhD student (-1987)
Straight Walk of Biped Walking Machine(1982)
Knee joint
Ankle joint
Toe
Heel
Hip joint
Leg Mechanism with 1 DOF
Ankle path generator(Planar 6-bar link mech)
Foot driving mechanism(Planar 8-bar link mech)
Driving links
Planar 8-bar link mechanism
Counter weightDrawing 8 figure
Plane-cylinder joint
Slanting crank shaft
Attitude Stabilizing Mechanism
950m
m
A Biped Waking Machine with only 1 DOF (Mechanical synthesis for simple control)
Walking speed 120 stepsmin (World record)
2nd prototype which can startand stop walking
Improvement for non steady walkingChange strideIndependent drive of foot driving mechanismArbitrary control of attitude stabilizing mechanismA new prototype with 5 DOF
Items Prototype-1 Prototype-2Degree-of-freedom 1 5Weight 206kgf 488kgfStraight walk Max 120stepmin Max15 stepsminStartstop to walk - Sucessful
Dynamics is important not only to stabilize biped machine but also to drive actuators
Lecture syllabus
Course title Advanced Mechanical ElementsAcademic major Mechanical EngineeringOffered quarter 2QDayperiod Thursday 3-4Credits 1-0-0Lecture room ZOOMRegistration number MECHH431Lecturer Prof Nobuyuki Iwatsuki
Office I1-305E-mail iwatsukinaamtitechacjp
Course description and aimsThis course offers the knowledge on kinematic and dynamic
analysis of planar and spatial link mechanisms and methods to control redundant and overunderactuated mechanisms
The characteristic of motion mechanisms affects the performance of advanced mechanical systems such as robots It is thus required to quantitatively evaluate the characteristic and to design and control mechanisms based on the evaluation criteria
For this purpose students will understand how to analyze the kinematics and dynamics of planar and spatial closed-loop link mechanisms by utilizing the systematic kinematic analysis method and will experience to apply them to kinematic and dynamic analyses and motion control of redundant mechanisms overactuated mechanisms of which actuator inputs are more than mobility of mechanism and underactuated mechanisms constrained by elastic elements or gravitational force
Student learning outcomesBy the end of this course students will be able to
(1)Explain mobility of mechanism and relation between inputoutput motion of mechanism
(2)Analyze displacement velocity and acceleration of planarspatialclosed-loop link mechanisms with the systematic kinematic analysis method
(3)Analyze the dynamics of planarspatial closed-loop link mechanisms utilizing the systematic kinematic analysis method
(4)Explain the optimum motion control of redundant link mechanisms
(5)Explain motion control of overactuated or underactuatedmechanisms with elastic elements
KeywordsClose-loop link mechanism kinematics dynamics systematic kinematic analysis motion control optimum control redundancy overactuated mechanism underactuated mechanism wire-driven mechanism
Class flowImportant issues are summarized at the end of lecture every week Because sample software to analyze mechanisms which are explained in this lecture will be offered to students students are expected to try to analyze various mechanisms with the software by themselves so as to understand what they learn
ScheduleClass 1 Kinematic analysis of planar link mechanism with the (June 25) systematic kinematic analysis method
- Displacement velocity and acceleration analyses of planar closed-loop link mechanism with the systematic kinematic analysis method -
Class 2 Kinematic analysis of spatial link mechanism with the(July 2) systematic kinematic analysis method
- Expansion of the systematic kinematic analysis method to spatial mechanisms -
Class 3 Dynamic analysis of planarspatial link mechanisms (July 9) - Driving force and joint force analysis using the
systematic kinematic analysis -Class 4 Motion control of redundant link mechanisms(July 16) - Optimum motion control to utilize redundancy -
Class 5 Kinematic analysis and motion control of overactuated(July 23) mechanisms
- Motion control of overactuated mechanisms using relaxation with elastic elements -
Class 6 Kinematic analysis and motion control of underactuated(July 30) mechanisms with elastic elements
- Motion control of underactuated mechanisms constrained by elastic elements -
Class 7 Kinematic analysis and motion control of underactuated(August 6) wire-driven mechanisms
- Motion control of wire-driven underactuatedmechanisms under gravitational force-
Textbook reference book course materialNo textbook is required Several handouts will be distributedvia OCW-i Students should download and print the handouts before class The following book is recommended as a reference book
JSME Textbook series Kinematics of Machineryedited by the Japan Society of Mechanical Engineers (2007)
Assessment criteria and methodsStudents knowledge of kinematic and dynamic analyses of planarspatial closed-loop link mechanisms with the systematic kinematic analysis method and their application to analyses and motion controls of redundant and underoveractuated mechanisms is assessed via submitted reports on several issues
Related coursesSynthesis of robotic mechanisms Design of robot controller
PrerequisitesStudents must have successfully completed Mechanical elements and draftings or have equivalent knowledge
June 25 2020
Advanced Mechanical Elements(Lecture 1)
Kinematic analysis of planar link mechanism with the systematic kinematic analysis method
Tokyo Institute of TechnologyDept of Mechanical EngineeringSchool of Engineering
Prof Nobuyuki Iwatsuki
1 Introduction of lecturer
Please visit my WEB-sitehttpwwwdynamicsmeptitechacjpindexphp
orSearch by Google with a keyword lsquoIwatsuki labrsquo
Nobuyuki Iwatsuki Professor Dr EngAffiliation
Dynamics Engineering LaboratoryDepartment of Mechanical EngineeringSchool of EngineeringTokyo Institute of Technology
OfficeRoom 305 Ishikawadai-1 Building Ohokayama Campus
Contact Tel +81-3-5734-2538Fax +81-3-5734-3917 (Department office)E-mail nobmeptitechacjp
Several artistic works
Research Themes(A)Synthesis and Control of Robotic Mechanisms
Especially focused on lsquoMechanism Design and Motion Control of Hyper RedundantUnderactuatedRobotsrsquo
(B)Silent EngineeringEspecially focused on lsquoEstimation of Sound Power Radiating from Vibrating Structure and Structural Optimization to Reduce the Sound Powerrsquo
(C)Functional Material ActuatorsEspecially focused on lsquoDevelopment of Micro CiliaryActuators in Grouprsquo
Several Examples of My Research Works(1)Biped Walking Machine
ndash While undergraduatePhD student (-1987)
Straight Walk of Biped Walking Machine(1982)
Knee joint
Ankle joint
Toe
Heel
Hip joint
Leg Mechanism with 1 DOF
Ankle path generator(Planar 6-bar link mech)
Foot driving mechanism(Planar 8-bar link mech)
Driving links
Planar 8-bar link mechanism
Counter weightDrawing 8 figure
Plane-cylinder joint
Slanting crank shaft
Attitude Stabilizing Mechanism
950m
m
A Biped Waking Machine with only 1 DOF (Mechanical synthesis for simple control)
Walking speed 120 stepsmin (World record)
2nd prototype which can startand stop walking
Improvement for non steady walkingChange strideIndependent drive of foot driving mechanismArbitrary control of attitude stabilizing mechanismA new prototype with 5 DOF
Items Prototype-1 Prototype-2Degree-of-freedom 1 5Weight 206kgf 488kgfStraight walk Max 120stepmin Max15 stepsminStartstop to walk - Sucessful
Dynamics is important not only to stabilize biped machine but also to drive actuators
Course description and aimsThis course offers the knowledge on kinematic and dynamic
analysis of planar and spatial link mechanisms and methods to control redundant and overunderactuated mechanisms
The characteristic of motion mechanisms affects the performance of advanced mechanical systems such as robots It is thus required to quantitatively evaluate the characteristic and to design and control mechanisms based on the evaluation criteria
For this purpose students will understand how to analyze the kinematics and dynamics of planar and spatial closed-loop link mechanisms by utilizing the systematic kinematic analysis method and will experience to apply them to kinematic and dynamic analyses and motion control of redundant mechanisms overactuated mechanisms of which actuator inputs are more than mobility of mechanism and underactuated mechanisms constrained by elastic elements or gravitational force
Student learning outcomesBy the end of this course students will be able to
(1)Explain mobility of mechanism and relation between inputoutput motion of mechanism
(2)Analyze displacement velocity and acceleration of planarspatialclosed-loop link mechanisms with the systematic kinematic analysis method
(3)Analyze the dynamics of planarspatial closed-loop link mechanisms utilizing the systematic kinematic analysis method
(4)Explain the optimum motion control of redundant link mechanisms
(5)Explain motion control of overactuated or underactuatedmechanisms with elastic elements
KeywordsClose-loop link mechanism kinematics dynamics systematic kinematic analysis motion control optimum control redundancy overactuated mechanism underactuated mechanism wire-driven mechanism
Class flowImportant issues are summarized at the end of lecture every week Because sample software to analyze mechanisms which are explained in this lecture will be offered to students students are expected to try to analyze various mechanisms with the software by themselves so as to understand what they learn
ScheduleClass 1 Kinematic analysis of planar link mechanism with the (June 25) systematic kinematic analysis method
- Displacement velocity and acceleration analyses of planar closed-loop link mechanism with the systematic kinematic analysis method -
Class 2 Kinematic analysis of spatial link mechanism with the(July 2) systematic kinematic analysis method
- Expansion of the systematic kinematic analysis method to spatial mechanisms -
Class 3 Dynamic analysis of planarspatial link mechanisms (July 9) - Driving force and joint force analysis using the
systematic kinematic analysis -Class 4 Motion control of redundant link mechanisms(July 16) - Optimum motion control to utilize redundancy -
Class 5 Kinematic analysis and motion control of overactuated(July 23) mechanisms
- Motion control of overactuated mechanisms using relaxation with elastic elements -
Class 6 Kinematic analysis and motion control of underactuated(July 30) mechanisms with elastic elements
- Motion control of underactuated mechanisms constrained by elastic elements -
Class 7 Kinematic analysis and motion control of underactuated(August 6) wire-driven mechanisms
- Motion control of wire-driven underactuatedmechanisms under gravitational force-
Textbook reference book course materialNo textbook is required Several handouts will be distributedvia OCW-i Students should download and print the handouts before class The following book is recommended as a reference book
JSME Textbook series Kinematics of Machineryedited by the Japan Society of Mechanical Engineers (2007)
Assessment criteria and methodsStudents knowledge of kinematic and dynamic analyses of planarspatial closed-loop link mechanisms with the systematic kinematic analysis method and their application to analyses and motion controls of redundant and underoveractuated mechanisms is assessed via submitted reports on several issues
Related coursesSynthesis of robotic mechanisms Design of robot controller
PrerequisitesStudents must have successfully completed Mechanical elements and draftings or have equivalent knowledge
June 25 2020
Advanced Mechanical Elements(Lecture 1)
Kinematic analysis of planar link mechanism with the systematic kinematic analysis method
Tokyo Institute of TechnologyDept of Mechanical EngineeringSchool of Engineering
Prof Nobuyuki Iwatsuki
1 Introduction of lecturer
Please visit my WEB-sitehttpwwwdynamicsmeptitechacjpindexphp
orSearch by Google with a keyword lsquoIwatsuki labrsquo
Nobuyuki Iwatsuki Professor Dr EngAffiliation
Dynamics Engineering LaboratoryDepartment of Mechanical EngineeringSchool of EngineeringTokyo Institute of Technology
OfficeRoom 305 Ishikawadai-1 Building Ohokayama Campus
Contact Tel +81-3-5734-2538Fax +81-3-5734-3917 (Department office)E-mail nobmeptitechacjp
Several artistic works
Research Themes(A)Synthesis and Control of Robotic Mechanisms
Especially focused on lsquoMechanism Design and Motion Control of Hyper RedundantUnderactuatedRobotsrsquo
(B)Silent EngineeringEspecially focused on lsquoEstimation of Sound Power Radiating from Vibrating Structure and Structural Optimization to Reduce the Sound Powerrsquo
(C)Functional Material ActuatorsEspecially focused on lsquoDevelopment of Micro CiliaryActuators in Grouprsquo
Several Examples of My Research Works(1)Biped Walking Machine
ndash While undergraduatePhD student (-1987)
Straight Walk of Biped Walking Machine(1982)
Knee joint
Ankle joint
Toe
Heel
Hip joint
Leg Mechanism with 1 DOF
Ankle path generator(Planar 6-bar link mech)
Foot driving mechanism(Planar 8-bar link mech)
Driving links
Planar 8-bar link mechanism
Counter weightDrawing 8 figure
Plane-cylinder joint
Slanting crank shaft
Attitude Stabilizing Mechanism
950m
m
A Biped Waking Machine with only 1 DOF (Mechanical synthesis for simple control)
Walking speed 120 stepsmin (World record)
2nd prototype which can startand stop walking
Improvement for non steady walkingChange strideIndependent drive of foot driving mechanismArbitrary control of attitude stabilizing mechanismA new prototype with 5 DOF
Items Prototype-1 Prototype-2Degree-of-freedom 1 5Weight 206kgf 488kgfStraight walk Max 120stepmin Max15 stepsminStartstop to walk - Sucessful
Dynamics is important not only to stabilize biped machine but also to drive actuators
Student learning outcomesBy the end of this course students will be able to
(1)Explain mobility of mechanism and relation between inputoutput motion of mechanism
(2)Analyze displacement velocity and acceleration of planarspatialclosed-loop link mechanisms with the systematic kinematic analysis method
(3)Analyze the dynamics of planarspatial closed-loop link mechanisms utilizing the systematic kinematic analysis method
(4)Explain the optimum motion control of redundant link mechanisms
(5)Explain motion control of overactuated or underactuatedmechanisms with elastic elements
KeywordsClose-loop link mechanism kinematics dynamics systematic kinematic analysis motion control optimum control redundancy overactuated mechanism underactuated mechanism wire-driven mechanism
Class flowImportant issues are summarized at the end of lecture every week Because sample software to analyze mechanisms which are explained in this lecture will be offered to students students are expected to try to analyze various mechanisms with the software by themselves so as to understand what they learn
ScheduleClass 1 Kinematic analysis of planar link mechanism with the (June 25) systematic kinematic analysis method
- Displacement velocity and acceleration analyses of planar closed-loop link mechanism with the systematic kinematic analysis method -
Class 2 Kinematic analysis of spatial link mechanism with the(July 2) systematic kinematic analysis method
- Expansion of the systematic kinematic analysis method to spatial mechanisms -
Class 3 Dynamic analysis of planarspatial link mechanisms (July 9) - Driving force and joint force analysis using the
systematic kinematic analysis -Class 4 Motion control of redundant link mechanisms(July 16) - Optimum motion control to utilize redundancy -
Class 5 Kinematic analysis and motion control of overactuated(July 23) mechanisms
- Motion control of overactuated mechanisms using relaxation with elastic elements -
Class 6 Kinematic analysis and motion control of underactuated(July 30) mechanisms with elastic elements
- Motion control of underactuated mechanisms constrained by elastic elements -
Class 7 Kinematic analysis and motion control of underactuated(August 6) wire-driven mechanisms
- Motion control of wire-driven underactuatedmechanisms under gravitational force-
Textbook reference book course materialNo textbook is required Several handouts will be distributedvia OCW-i Students should download and print the handouts before class The following book is recommended as a reference book
JSME Textbook series Kinematics of Machineryedited by the Japan Society of Mechanical Engineers (2007)
Assessment criteria and methodsStudents knowledge of kinematic and dynamic analyses of planarspatial closed-loop link mechanisms with the systematic kinematic analysis method and their application to analyses and motion controls of redundant and underoveractuated mechanisms is assessed via submitted reports on several issues
Related coursesSynthesis of robotic mechanisms Design of robot controller
PrerequisitesStudents must have successfully completed Mechanical elements and draftings or have equivalent knowledge
June 25 2020
Advanced Mechanical Elements(Lecture 1)
Kinematic analysis of planar link mechanism with the systematic kinematic analysis method
Tokyo Institute of TechnologyDept of Mechanical EngineeringSchool of Engineering
Prof Nobuyuki Iwatsuki
1 Introduction of lecturer
Please visit my WEB-sitehttpwwwdynamicsmeptitechacjpindexphp
orSearch by Google with a keyword lsquoIwatsuki labrsquo
Nobuyuki Iwatsuki Professor Dr EngAffiliation
Dynamics Engineering LaboratoryDepartment of Mechanical EngineeringSchool of EngineeringTokyo Institute of Technology
OfficeRoom 305 Ishikawadai-1 Building Ohokayama Campus
Contact Tel +81-3-5734-2538Fax +81-3-5734-3917 (Department office)E-mail nobmeptitechacjp
Several artistic works
Research Themes(A)Synthesis and Control of Robotic Mechanisms
Especially focused on lsquoMechanism Design and Motion Control of Hyper RedundantUnderactuatedRobotsrsquo
(B)Silent EngineeringEspecially focused on lsquoEstimation of Sound Power Radiating from Vibrating Structure and Structural Optimization to Reduce the Sound Powerrsquo
(C)Functional Material ActuatorsEspecially focused on lsquoDevelopment of Micro CiliaryActuators in Grouprsquo
Several Examples of My Research Works(1)Biped Walking Machine
ndash While undergraduatePhD student (-1987)
Straight Walk of Biped Walking Machine(1982)
Knee joint
Ankle joint
Toe
Heel
Hip joint
Leg Mechanism with 1 DOF
Ankle path generator(Planar 6-bar link mech)
Foot driving mechanism(Planar 8-bar link mech)
Driving links
Planar 8-bar link mechanism
Counter weightDrawing 8 figure
Plane-cylinder joint
Slanting crank shaft
Attitude Stabilizing Mechanism
950m
m
A Biped Waking Machine with only 1 DOF (Mechanical synthesis for simple control)
Walking speed 120 stepsmin (World record)
2nd prototype which can startand stop walking
Improvement for non steady walkingChange strideIndependent drive of foot driving mechanismArbitrary control of attitude stabilizing mechanismA new prototype with 5 DOF
Items Prototype-1 Prototype-2Degree-of-freedom 1 5Weight 206kgf 488kgfStraight walk Max 120stepmin Max15 stepsminStartstop to walk - Sucessful
Dynamics is important not only to stabilize biped machine but also to drive actuators
KeywordsClose-loop link mechanism kinematics dynamics systematic kinematic analysis motion control optimum control redundancy overactuated mechanism underactuated mechanism wire-driven mechanism
Class flowImportant issues are summarized at the end of lecture every week Because sample software to analyze mechanisms which are explained in this lecture will be offered to students students are expected to try to analyze various mechanisms with the software by themselves so as to understand what they learn
ScheduleClass 1 Kinematic analysis of planar link mechanism with the (June 25) systematic kinematic analysis method
- Displacement velocity and acceleration analyses of planar closed-loop link mechanism with the systematic kinematic analysis method -
Class 2 Kinematic analysis of spatial link mechanism with the(July 2) systematic kinematic analysis method
- Expansion of the systematic kinematic analysis method to spatial mechanisms -
Class 3 Dynamic analysis of planarspatial link mechanisms (July 9) - Driving force and joint force analysis using the
systematic kinematic analysis -Class 4 Motion control of redundant link mechanisms(July 16) - Optimum motion control to utilize redundancy -
Class 5 Kinematic analysis and motion control of overactuated(July 23) mechanisms
- Motion control of overactuated mechanisms using relaxation with elastic elements -
Class 6 Kinematic analysis and motion control of underactuated(July 30) mechanisms with elastic elements
- Motion control of underactuated mechanisms constrained by elastic elements -
Class 7 Kinematic analysis and motion control of underactuated(August 6) wire-driven mechanisms
- Motion control of wire-driven underactuatedmechanisms under gravitational force-
Textbook reference book course materialNo textbook is required Several handouts will be distributedvia OCW-i Students should download and print the handouts before class The following book is recommended as a reference book
JSME Textbook series Kinematics of Machineryedited by the Japan Society of Mechanical Engineers (2007)
Assessment criteria and methodsStudents knowledge of kinematic and dynamic analyses of planarspatial closed-loop link mechanisms with the systematic kinematic analysis method and their application to analyses and motion controls of redundant and underoveractuated mechanisms is assessed via submitted reports on several issues
Related coursesSynthesis of robotic mechanisms Design of robot controller
PrerequisitesStudents must have successfully completed Mechanical elements and draftings or have equivalent knowledge
June 25 2020
Advanced Mechanical Elements(Lecture 1)
Kinematic analysis of planar link mechanism with the systematic kinematic analysis method
Tokyo Institute of TechnologyDept of Mechanical EngineeringSchool of Engineering
Prof Nobuyuki Iwatsuki
1 Introduction of lecturer
Please visit my WEB-sitehttpwwwdynamicsmeptitechacjpindexphp
orSearch by Google with a keyword lsquoIwatsuki labrsquo
Nobuyuki Iwatsuki Professor Dr EngAffiliation
Dynamics Engineering LaboratoryDepartment of Mechanical EngineeringSchool of EngineeringTokyo Institute of Technology
OfficeRoom 305 Ishikawadai-1 Building Ohokayama Campus
Contact Tel +81-3-5734-2538Fax +81-3-5734-3917 (Department office)E-mail nobmeptitechacjp
Several artistic works
Research Themes(A)Synthesis and Control of Robotic Mechanisms
Especially focused on lsquoMechanism Design and Motion Control of Hyper RedundantUnderactuatedRobotsrsquo
(B)Silent EngineeringEspecially focused on lsquoEstimation of Sound Power Radiating from Vibrating Structure and Structural Optimization to Reduce the Sound Powerrsquo
(C)Functional Material ActuatorsEspecially focused on lsquoDevelopment of Micro CiliaryActuators in Grouprsquo
Several Examples of My Research Works(1)Biped Walking Machine
ndash While undergraduatePhD student (-1987)
Straight Walk of Biped Walking Machine(1982)
Knee joint
Ankle joint
Toe
Heel
Hip joint
Leg Mechanism with 1 DOF
Ankle path generator(Planar 6-bar link mech)
Foot driving mechanism(Planar 8-bar link mech)
Driving links
Planar 8-bar link mechanism
Counter weightDrawing 8 figure
Plane-cylinder joint
Slanting crank shaft
Attitude Stabilizing Mechanism
950m
m
A Biped Waking Machine with only 1 DOF (Mechanical synthesis for simple control)
Walking speed 120 stepsmin (World record)
2nd prototype which can startand stop walking
Improvement for non steady walkingChange strideIndependent drive of foot driving mechanismArbitrary control of attitude stabilizing mechanismA new prototype with 5 DOF
Items Prototype-1 Prototype-2Degree-of-freedom 1 5Weight 206kgf 488kgfStraight walk Max 120stepmin Max15 stepsminStartstop to walk - Sucessful
Dynamics is important not only to stabilize biped machine but also to drive actuators
ScheduleClass 1 Kinematic analysis of planar link mechanism with the (June 25) systematic kinematic analysis method
- Displacement velocity and acceleration analyses of planar closed-loop link mechanism with the systematic kinematic analysis method -
Class 2 Kinematic analysis of spatial link mechanism with the(July 2) systematic kinematic analysis method
- Expansion of the systematic kinematic analysis method to spatial mechanisms -
Class 3 Dynamic analysis of planarspatial link mechanisms (July 9) - Driving force and joint force analysis using the
systematic kinematic analysis -Class 4 Motion control of redundant link mechanisms(July 16) - Optimum motion control to utilize redundancy -
Class 5 Kinematic analysis and motion control of overactuated(July 23) mechanisms
- Motion control of overactuated mechanisms using relaxation with elastic elements -
Class 6 Kinematic analysis and motion control of underactuated(July 30) mechanisms with elastic elements
- Motion control of underactuated mechanisms constrained by elastic elements -
Class 7 Kinematic analysis and motion control of underactuated(August 6) wire-driven mechanisms
- Motion control of wire-driven underactuatedmechanisms under gravitational force-
Textbook reference book course materialNo textbook is required Several handouts will be distributedvia OCW-i Students should download and print the handouts before class The following book is recommended as a reference book
JSME Textbook series Kinematics of Machineryedited by the Japan Society of Mechanical Engineers (2007)
Assessment criteria and methodsStudents knowledge of kinematic and dynamic analyses of planarspatial closed-loop link mechanisms with the systematic kinematic analysis method and their application to analyses and motion controls of redundant and underoveractuated mechanisms is assessed via submitted reports on several issues
Related coursesSynthesis of robotic mechanisms Design of robot controller
PrerequisitesStudents must have successfully completed Mechanical elements and draftings or have equivalent knowledge
June 25 2020
Advanced Mechanical Elements(Lecture 1)
Kinematic analysis of planar link mechanism with the systematic kinematic analysis method
Tokyo Institute of TechnologyDept of Mechanical EngineeringSchool of Engineering
Prof Nobuyuki Iwatsuki
1 Introduction of lecturer
Please visit my WEB-sitehttpwwwdynamicsmeptitechacjpindexphp
orSearch by Google with a keyword lsquoIwatsuki labrsquo
Nobuyuki Iwatsuki Professor Dr EngAffiliation
Dynamics Engineering LaboratoryDepartment of Mechanical EngineeringSchool of EngineeringTokyo Institute of Technology
OfficeRoom 305 Ishikawadai-1 Building Ohokayama Campus
Contact Tel +81-3-5734-2538Fax +81-3-5734-3917 (Department office)E-mail nobmeptitechacjp
Several artistic works
Research Themes(A)Synthesis and Control of Robotic Mechanisms
Especially focused on lsquoMechanism Design and Motion Control of Hyper RedundantUnderactuatedRobotsrsquo
(B)Silent EngineeringEspecially focused on lsquoEstimation of Sound Power Radiating from Vibrating Structure and Structural Optimization to Reduce the Sound Powerrsquo
(C)Functional Material ActuatorsEspecially focused on lsquoDevelopment of Micro CiliaryActuators in Grouprsquo
Several Examples of My Research Works(1)Biped Walking Machine
ndash While undergraduatePhD student (-1987)
Straight Walk of Biped Walking Machine(1982)
Knee joint
Ankle joint
Toe
Heel
Hip joint
Leg Mechanism with 1 DOF
Ankle path generator(Planar 6-bar link mech)
Foot driving mechanism(Planar 8-bar link mech)
Driving links
Planar 8-bar link mechanism
Counter weightDrawing 8 figure
Plane-cylinder joint
Slanting crank shaft
Attitude Stabilizing Mechanism
950m
m
A Biped Waking Machine with only 1 DOF (Mechanical synthesis for simple control)
Walking speed 120 stepsmin (World record)
2nd prototype which can startand stop walking
Improvement for non steady walkingChange strideIndependent drive of foot driving mechanismArbitrary control of attitude stabilizing mechanismA new prototype with 5 DOF
Items Prototype-1 Prototype-2Degree-of-freedom 1 5Weight 206kgf 488kgfStraight walk Max 120stepmin Max15 stepsminStartstop to walk - Sucessful
Dynamics is important not only to stabilize biped machine but also to drive actuators
Class 5 Kinematic analysis and motion control of overactuated(July 23) mechanisms
- Motion control of overactuated mechanisms using relaxation with elastic elements -
Class 6 Kinematic analysis and motion control of underactuated(July 30) mechanisms with elastic elements
- Motion control of underactuated mechanisms constrained by elastic elements -
Class 7 Kinematic analysis and motion control of underactuated(August 6) wire-driven mechanisms
- Motion control of wire-driven underactuatedmechanisms under gravitational force-
Textbook reference book course materialNo textbook is required Several handouts will be distributedvia OCW-i Students should download and print the handouts before class The following book is recommended as a reference book
JSME Textbook series Kinematics of Machineryedited by the Japan Society of Mechanical Engineers (2007)
Assessment criteria and methodsStudents knowledge of kinematic and dynamic analyses of planarspatial closed-loop link mechanisms with the systematic kinematic analysis method and their application to analyses and motion controls of redundant and underoveractuated mechanisms is assessed via submitted reports on several issues
Related coursesSynthesis of robotic mechanisms Design of robot controller
PrerequisitesStudents must have successfully completed Mechanical elements and draftings or have equivalent knowledge
June 25 2020
Advanced Mechanical Elements(Lecture 1)
Kinematic analysis of planar link mechanism with the systematic kinematic analysis method
Tokyo Institute of TechnologyDept of Mechanical EngineeringSchool of Engineering
Prof Nobuyuki Iwatsuki
1 Introduction of lecturer
Please visit my WEB-sitehttpwwwdynamicsmeptitechacjpindexphp
orSearch by Google with a keyword lsquoIwatsuki labrsquo
Nobuyuki Iwatsuki Professor Dr EngAffiliation
Dynamics Engineering LaboratoryDepartment of Mechanical EngineeringSchool of EngineeringTokyo Institute of Technology
OfficeRoom 305 Ishikawadai-1 Building Ohokayama Campus
Contact Tel +81-3-5734-2538Fax +81-3-5734-3917 (Department office)E-mail nobmeptitechacjp
Several artistic works
Research Themes(A)Synthesis and Control of Robotic Mechanisms
Especially focused on lsquoMechanism Design and Motion Control of Hyper RedundantUnderactuatedRobotsrsquo
(B)Silent EngineeringEspecially focused on lsquoEstimation of Sound Power Radiating from Vibrating Structure and Structural Optimization to Reduce the Sound Powerrsquo
(C)Functional Material ActuatorsEspecially focused on lsquoDevelopment of Micro CiliaryActuators in Grouprsquo
Several Examples of My Research Works(1)Biped Walking Machine
ndash While undergraduatePhD student (-1987)
Straight Walk of Biped Walking Machine(1982)
Knee joint
Ankle joint
Toe
Heel
Hip joint
Leg Mechanism with 1 DOF
Ankle path generator(Planar 6-bar link mech)
Foot driving mechanism(Planar 8-bar link mech)
Driving links
Planar 8-bar link mechanism
Counter weightDrawing 8 figure
Plane-cylinder joint
Slanting crank shaft
Attitude Stabilizing Mechanism
950m
m
A Biped Waking Machine with only 1 DOF (Mechanical synthesis for simple control)
Walking speed 120 stepsmin (World record)
2nd prototype which can startand stop walking
Improvement for non steady walkingChange strideIndependent drive of foot driving mechanismArbitrary control of attitude stabilizing mechanismA new prototype with 5 DOF
Items Prototype-1 Prototype-2Degree-of-freedom 1 5Weight 206kgf 488kgfStraight walk Max 120stepmin Max15 stepsminStartstop to walk - Sucessful
Dynamics is important not only to stabilize biped machine but also to drive actuators
Textbook reference book course materialNo textbook is required Several handouts will be distributedvia OCW-i Students should download and print the handouts before class The following book is recommended as a reference book
JSME Textbook series Kinematics of Machineryedited by the Japan Society of Mechanical Engineers (2007)
Assessment criteria and methodsStudents knowledge of kinematic and dynamic analyses of planarspatial closed-loop link mechanisms with the systematic kinematic analysis method and their application to analyses and motion controls of redundant and underoveractuated mechanisms is assessed via submitted reports on several issues
Related coursesSynthesis of robotic mechanisms Design of robot controller
PrerequisitesStudents must have successfully completed Mechanical elements and draftings or have equivalent knowledge
June 25 2020
Advanced Mechanical Elements(Lecture 1)
Kinematic analysis of planar link mechanism with the systematic kinematic analysis method
Tokyo Institute of TechnologyDept of Mechanical EngineeringSchool of Engineering
Prof Nobuyuki Iwatsuki
1 Introduction of lecturer
Please visit my WEB-sitehttpwwwdynamicsmeptitechacjpindexphp
orSearch by Google with a keyword lsquoIwatsuki labrsquo
Nobuyuki Iwatsuki Professor Dr EngAffiliation
Dynamics Engineering LaboratoryDepartment of Mechanical EngineeringSchool of EngineeringTokyo Institute of Technology
OfficeRoom 305 Ishikawadai-1 Building Ohokayama Campus
Contact Tel +81-3-5734-2538Fax +81-3-5734-3917 (Department office)E-mail nobmeptitechacjp
Several artistic works
Research Themes(A)Synthesis and Control of Robotic Mechanisms
Especially focused on lsquoMechanism Design and Motion Control of Hyper RedundantUnderactuatedRobotsrsquo
(B)Silent EngineeringEspecially focused on lsquoEstimation of Sound Power Radiating from Vibrating Structure and Structural Optimization to Reduce the Sound Powerrsquo
(C)Functional Material ActuatorsEspecially focused on lsquoDevelopment of Micro CiliaryActuators in Grouprsquo
Several Examples of My Research Works(1)Biped Walking Machine
ndash While undergraduatePhD student (-1987)
Straight Walk of Biped Walking Machine(1982)
Knee joint
Ankle joint
Toe
Heel
Hip joint
Leg Mechanism with 1 DOF
Ankle path generator(Planar 6-bar link mech)
Foot driving mechanism(Planar 8-bar link mech)
Driving links
Planar 8-bar link mechanism
Counter weightDrawing 8 figure
Plane-cylinder joint
Slanting crank shaft
Attitude Stabilizing Mechanism
950m
m
A Biped Waking Machine with only 1 DOF (Mechanical synthesis for simple control)
Walking speed 120 stepsmin (World record)
2nd prototype which can startand stop walking
Improvement for non steady walkingChange strideIndependent drive of foot driving mechanismArbitrary control of attitude stabilizing mechanismA new prototype with 5 DOF
Items Prototype-1 Prototype-2Degree-of-freedom 1 5Weight 206kgf 488kgfStraight walk Max 120stepmin Max15 stepsminStartstop to walk - Sucessful
Dynamics is important not only to stabilize biped machine but also to drive actuators
June 25 2020
Advanced Mechanical Elements(Lecture 1)
Kinematic analysis of planar link mechanism with the systematic kinematic analysis method
Tokyo Institute of TechnologyDept of Mechanical EngineeringSchool of Engineering
Prof Nobuyuki Iwatsuki
1 Introduction of lecturer
Please visit my WEB-sitehttpwwwdynamicsmeptitechacjpindexphp
orSearch by Google with a keyword lsquoIwatsuki labrsquo
Nobuyuki Iwatsuki Professor Dr EngAffiliation
Dynamics Engineering LaboratoryDepartment of Mechanical EngineeringSchool of EngineeringTokyo Institute of Technology
OfficeRoom 305 Ishikawadai-1 Building Ohokayama Campus
Contact Tel +81-3-5734-2538Fax +81-3-5734-3917 (Department office)E-mail nobmeptitechacjp
Several artistic works
Research Themes(A)Synthesis and Control of Robotic Mechanisms
Especially focused on lsquoMechanism Design and Motion Control of Hyper RedundantUnderactuatedRobotsrsquo
(B)Silent EngineeringEspecially focused on lsquoEstimation of Sound Power Radiating from Vibrating Structure and Structural Optimization to Reduce the Sound Powerrsquo
(C)Functional Material ActuatorsEspecially focused on lsquoDevelopment of Micro CiliaryActuators in Grouprsquo
Several Examples of My Research Works(1)Biped Walking Machine
ndash While undergraduatePhD student (-1987)
Straight Walk of Biped Walking Machine(1982)
Knee joint
Ankle joint
Toe
Heel
Hip joint
Leg Mechanism with 1 DOF
Ankle path generator(Planar 6-bar link mech)
Foot driving mechanism(Planar 8-bar link mech)
Driving links
Planar 8-bar link mechanism
Counter weightDrawing 8 figure
Plane-cylinder joint
Slanting crank shaft
Attitude Stabilizing Mechanism
950m
m
A Biped Waking Machine with only 1 DOF (Mechanical synthesis for simple control)
Walking speed 120 stepsmin (World record)
2nd prototype which can startand stop walking
Improvement for non steady walkingChange strideIndependent drive of foot driving mechanismArbitrary control of attitude stabilizing mechanismA new prototype with 5 DOF
Items Prototype-1 Prototype-2Degree-of-freedom 1 5Weight 206kgf 488kgfStraight walk Max 120stepmin Max15 stepsminStartstop to walk - Sucessful
Dynamics is important not only to stabilize biped machine but also to drive actuators
1 Introduction of lecturer
Please visit my WEB-sitehttpwwwdynamicsmeptitechacjpindexphp
orSearch by Google with a keyword lsquoIwatsuki labrsquo
Nobuyuki Iwatsuki Professor Dr EngAffiliation
Dynamics Engineering LaboratoryDepartment of Mechanical EngineeringSchool of EngineeringTokyo Institute of Technology
OfficeRoom 305 Ishikawadai-1 Building Ohokayama Campus
Contact Tel +81-3-5734-2538Fax +81-3-5734-3917 (Department office)E-mail nobmeptitechacjp
Several artistic works
Research Themes(A)Synthesis and Control of Robotic Mechanisms
Especially focused on lsquoMechanism Design and Motion Control of Hyper RedundantUnderactuatedRobotsrsquo
(B)Silent EngineeringEspecially focused on lsquoEstimation of Sound Power Radiating from Vibrating Structure and Structural Optimization to Reduce the Sound Powerrsquo
(C)Functional Material ActuatorsEspecially focused on lsquoDevelopment of Micro CiliaryActuators in Grouprsquo
Several Examples of My Research Works(1)Biped Walking Machine
ndash While undergraduatePhD student (-1987)
Straight Walk of Biped Walking Machine(1982)
Knee joint
Ankle joint
Toe
Heel
Hip joint
Leg Mechanism with 1 DOF
Ankle path generator(Planar 6-bar link mech)
Foot driving mechanism(Planar 8-bar link mech)
Driving links
Planar 8-bar link mechanism
Counter weightDrawing 8 figure
Plane-cylinder joint
Slanting crank shaft
Attitude Stabilizing Mechanism
950m
m
A Biped Waking Machine with only 1 DOF (Mechanical synthesis for simple control)
Walking speed 120 stepsmin (World record)
2nd prototype which can startand stop walking
Improvement for non steady walkingChange strideIndependent drive of foot driving mechanismArbitrary control of attitude stabilizing mechanismA new prototype with 5 DOF
Items Prototype-1 Prototype-2Degree-of-freedom 1 5Weight 206kgf 488kgfStraight walk Max 120stepmin Max15 stepsminStartstop to walk - Sucessful
Dynamics is important not only to stabilize biped machine but also to drive actuators
Nobuyuki Iwatsuki Professor Dr EngAffiliation
Dynamics Engineering LaboratoryDepartment of Mechanical EngineeringSchool of EngineeringTokyo Institute of Technology
OfficeRoom 305 Ishikawadai-1 Building Ohokayama Campus
Contact Tel +81-3-5734-2538Fax +81-3-5734-3917 (Department office)E-mail nobmeptitechacjp
Several artistic works
Research Themes(A)Synthesis and Control of Robotic Mechanisms
Especially focused on lsquoMechanism Design and Motion Control of Hyper RedundantUnderactuatedRobotsrsquo
(B)Silent EngineeringEspecially focused on lsquoEstimation of Sound Power Radiating from Vibrating Structure and Structural Optimization to Reduce the Sound Powerrsquo
(C)Functional Material ActuatorsEspecially focused on lsquoDevelopment of Micro CiliaryActuators in Grouprsquo
Several Examples of My Research Works(1)Biped Walking Machine
ndash While undergraduatePhD student (-1987)
Straight Walk of Biped Walking Machine(1982)
Knee joint
Ankle joint
Toe
Heel
Hip joint
Leg Mechanism with 1 DOF
Ankle path generator(Planar 6-bar link mech)
Foot driving mechanism(Planar 8-bar link mech)
Driving links
Planar 8-bar link mechanism
Counter weightDrawing 8 figure
Plane-cylinder joint
Slanting crank shaft
Attitude Stabilizing Mechanism
950m
m
A Biped Waking Machine with only 1 DOF (Mechanical synthesis for simple control)
Walking speed 120 stepsmin (World record)
2nd prototype which can startand stop walking
Improvement for non steady walkingChange strideIndependent drive of foot driving mechanismArbitrary control of attitude stabilizing mechanismA new prototype with 5 DOF
Items Prototype-1 Prototype-2Degree-of-freedom 1 5Weight 206kgf 488kgfStraight walk Max 120stepmin Max15 stepsminStartstop to walk - Sucessful
Dynamics is important not only to stabilize biped machine but also to drive actuators
Several artistic works
Research Themes(A)Synthesis and Control of Robotic Mechanisms
Especially focused on lsquoMechanism Design and Motion Control of Hyper RedundantUnderactuatedRobotsrsquo
(B)Silent EngineeringEspecially focused on lsquoEstimation of Sound Power Radiating from Vibrating Structure and Structural Optimization to Reduce the Sound Powerrsquo
(C)Functional Material ActuatorsEspecially focused on lsquoDevelopment of Micro CiliaryActuators in Grouprsquo
Several Examples of My Research Works(1)Biped Walking Machine
ndash While undergraduatePhD student (-1987)
Straight Walk of Biped Walking Machine(1982)
Knee joint
Ankle joint
Toe
Heel
Hip joint
Leg Mechanism with 1 DOF
Ankle path generator(Planar 6-bar link mech)
Foot driving mechanism(Planar 8-bar link mech)
Driving links
Planar 8-bar link mechanism
Counter weightDrawing 8 figure
Plane-cylinder joint
Slanting crank shaft
Attitude Stabilizing Mechanism
950m
m
A Biped Waking Machine with only 1 DOF (Mechanical synthesis for simple control)
Walking speed 120 stepsmin (World record)
2nd prototype which can startand stop walking
Improvement for non steady walkingChange strideIndependent drive of foot driving mechanismArbitrary control of attitude stabilizing mechanismA new prototype with 5 DOF
Items Prototype-1 Prototype-2Degree-of-freedom 1 5Weight 206kgf 488kgfStraight walk Max 120stepmin Max15 stepsminStartstop to walk - Sucessful
Dynamics is important not only to stabilize biped machine but also to drive actuators
Research Themes(A)Synthesis and Control of Robotic Mechanisms
Especially focused on lsquoMechanism Design and Motion Control of Hyper RedundantUnderactuatedRobotsrsquo
(B)Silent EngineeringEspecially focused on lsquoEstimation of Sound Power Radiating from Vibrating Structure and Structural Optimization to Reduce the Sound Powerrsquo
(C)Functional Material ActuatorsEspecially focused on lsquoDevelopment of Micro CiliaryActuators in Grouprsquo
Several Examples of My Research Works(1)Biped Walking Machine
ndash While undergraduatePhD student (-1987)
Straight Walk of Biped Walking Machine(1982)
Knee joint
Ankle joint
Toe
Heel
Hip joint
Leg Mechanism with 1 DOF
Ankle path generator(Planar 6-bar link mech)
Foot driving mechanism(Planar 8-bar link mech)
Driving links
Planar 8-bar link mechanism
Counter weightDrawing 8 figure
Plane-cylinder joint
Slanting crank shaft
Attitude Stabilizing Mechanism
950m
m
A Biped Waking Machine with only 1 DOF (Mechanical synthesis for simple control)
Walking speed 120 stepsmin (World record)
2nd prototype which can startand stop walking
Improvement for non steady walkingChange strideIndependent drive of foot driving mechanismArbitrary control of attitude stabilizing mechanismA new prototype with 5 DOF
Items Prototype-1 Prototype-2Degree-of-freedom 1 5Weight 206kgf 488kgfStraight walk Max 120stepmin Max15 stepsminStartstop to walk - Sucessful
Dynamics is important not only to stabilize biped machine but also to drive actuators
Several Examples of My Research Works(1)Biped Walking Machine
ndash While undergraduatePhD student (-1987)
Straight Walk of Biped Walking Machine(1982)
Knee joint
Ankle joint
Toe
Heel
Hip joint
Leg Mechanism with 1 DOF
Ankle path generator(Planar 6-bar link mech)
Foot driving mechanism(Planar 8-bar link mech)
Driving links
Planar 8-bar link mechanism
Counter weightDrawing 8 figure
Plane-cylinder joint
Slanting crank shaft
Attitude Stabilizing Mechanism
950m
m
A Biped Waking Machine with only 1 DOF (Mechanical synthesis for simple control)
Walking speed 120 stepsmin (World record)
2nd prototype which can startand stop walking
Improvement for non steady walkingChange strideIndependent drive of foot driving mechanismArbitrary control of attitude stabilizing mechanismA new prototype with 5 DOF
Items Prototype-1 Prototype-2Degree-of-freedom 1 5Weight 206kgf 488kgfStraight walk Max 120stepmin Max15 stepsminStartstop to walk - Sucessful
Dynamics is important not only to stabilize biped machine but also to drive actuators
Walking speed 120 stepsmin (World record)
2nd prototype which can startand stop walking
Improvement for non steady walkingChange strideIndependent drive of foot driving mechanismArbitrary control of attitude stabilizing mechanismA new prototype with 5 DOF
Items Prototype-1 Prototype-2Degree-of-freedom 1 5Weight 206kgf 488kgfStraight walk Max 120stepmin Max15 stepsminStartstop to walk - Sucessful
Dynamics is important not only to stabilize biped machine but also to drive actuators
(2)Dexterous Motion of Hyper Redundant Robot- Optimum learning control of snake-like robot
with 10DOF to avoid obstackles
Dexterity is set as an objective function togenerate optimum trajectory
(3)Control of Structural Flexibility and Its application
Output point
Motion control of a flexible link (coil spring)taking account of its reaction force
x z
y
Four legged walking machine with spring leg
(4)Estimation of Sound Radiation from Thin Platesubjected to Acoustic Excitation
Thin rectangular plate
Acoustic excitation
Transmitted sound power
Transmitted sound power should be estimated to designsilent structure
Pseudo random noise Impulsive signal
Transmitted sound power can be estimatedwith an adequate accuracy
Due to mode of vibration
Input sound pressure Input sound pressure
Transmitted sound powerTransmitted sound power
(5)Estimation of Sound Radiation from Frog-type Guiro
The calculated acceleration and sound pressureExpanded
(6)Development of Micro Ciliary Actuators in GroupBase plate
Micro machine driven by many artificial micro cilium actuators
Artificial cilium motion
Piezoelectric pipemorph actuator
Yy
2θ
L do
φdi
Elastic bar(Internal electrode)
Piezoelectric ceramics
External electrode
~
Piezoelectric Pipemorph actuator
Partial Ni plating with Watt solution bath
125μm
100μm
Fabrication of PZT film
~
Ni electrodes ~20kV
Internal electrodes
Vacuum desiccator Heater for soldering
Sn-Pb SolderNi plate
Several fabrication methods
Convey of a small sheet with micro cilium actuators in group
Microscope
Micro cilium actuators in group
A paper sheet 6mmtimes6mm
8μm
I hope you will visit my WEB-site
httpwwwdynamicsmeptitechacjpindexphp
if you are interested in other research works
Thank you
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Itrsquos easy to calculate output motion of themanipulator
2 Kinematics of planar serial link mechanisms
Forward kinematics
+++++minusminus
=
)cos()(sin)sin()(sin
10101000
10101000
θθθθθθθθθθθθ
LLLL
YX
P
P
By differentiating the displacement the output velocity can be calculated as
Linear relationship between output velocity and joint velocity
It can be deformed as
++++minus+minusminus
=
1
0
10110100
10110100
)cos()cos(cos)sin()sin(sin
θθ
θθθθθθθθθθ
LLLLLL
YX
P
P
Jacobian matrix J(θ) Joint velocityvector θ
Output velocityvector r
θθJr )(=
θθJθθJ
)()(
)]sin()()cos()([
)sincos()]cos()()sin()([
)cossin(
102
1010101
020000
102
1010101
020000
+=
++minus+++
minus+++++minus
+minus
=
θθθθθθθθ
θθθθθθθθθθθθ
θθθθ
L
LL
L
YX
P
P
Output acceleration can also be calculated as
I t is easy to analyze output motion with knowledge at high school
For general planar serial nR manipulatorhellip
+
+=
sum sum
sum summinus
= =
minus
= =
0
1
0 0
0
1
0 0
sin
cos
YL
XL
YX
n
i
i
kki
n
i
i
kki
P
P
θ
θ
minus
=
sum sumsum
sum sumsumminus
= ==
minus
= ==1
0 00
1
0 00
cos
sinn
i
i
kk
i
kki
n
i
i
kk
i
kki
P
P
L
L
YX
θθ
θθ
minus
+minus=
sum sum sum sumsum
sum sum sum sumsumminus
= = = ==
minus
= = = ==1
0 0 0 0
2
0
1
0 0 0 0
2
0
]sin)(cos[
]cos)(sin[n
i
i
k
i
k
i
kkkk
i
kki
n
i
i
k
i
k
i
kkkk
i
kki
P
P
L
L
YX
θθθθ
θθθθ
Itrsquos also easy to analyze
Inverse kinematics
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
To calculate joint input motion forthe specified output motion
Can θ0θ1 be calculated as the function of XPYP
It will be a little bit complicated
By using the prementioned equation for forward kinematics
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Let draw an auxiliary line
By defining angles αβ
βαθ minus=0
where
0
01tanXXYY
P
P
minusminus
= minusα
How to derive β X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
α
β
Planar serial 2R manipulator
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
β
Let consider PJ0J1
L
20
20 )()( YYXXL PP minus+minus=
By using side lengths L0L1 anddistance
0
21
20
2
2cos
LLLLL minus+
=β
We can apply cosine theorem
0
21
20
21
2cos
LLLLL minus+
= minusβTherefore
Planar serial 2R manipulator
0
21
20
21
0
01
0
2costan
LLLLL
XXYY
P
P minus+minus
minusminus
=
minus=
minusminus
βαθ
θ1 can then be calculated
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1 L
Let consider PJ0J1 and an angle γ
γ
θ1=πminusγ
10
221
201
2cos
LLLLL minus+
= minusγ
From cosine theorem
10
221
201
1
2cos
LLLLL minus+
minus=
minus=
minusπ
γπθ
Joint input velocity and acceleration can be derived as
γθ
γθ
βαθ
βαθ
minus=
minus=
minus=
minus=
1
1
0
0
γγ
sin10LLLL
=
γγγγγ 2
10
2
sincossin)(
LLLLLLL
minus+
=
LYYXXL PPPP
+
=
2
22 )()(L
LYYXXLYYYXXXL PPPPPPPPPP
+minus+++=
They can also be derived as
+minusminus+minusminus
++=
++++minus+minusminus
=
=minus
minus
P
P
P
P
YX
LLLLLL
LL
YX
LLLLLL
)sin(sin)cos(cos)sin()cos(
sin1
)cos()cos(cos)sin()sin(sin
)(
1010010100
101101
110
1
10110100
10110100
1
θθθθθθθθθθ
θ
θθθθθθθθθθ
rθJθ
++minus++minusminus++minus++minusminus
minus
times
+minusminus+minusminus
++=
minus= minus
1
0
1010110101000
1010110101000
1010010100
101101
110
1
)sin()()sin()(sin)cos()()cos()(cos
)sin(sin)cos(cos)sin()cos(
sin1
])([)(
θθ
θθθθθθθθθθθθθθθθθθθθ
θθθθθθθθθθ
θ
LLLLLL
YX
LLLLLL
LL
P
P
θθJrθJθ
Anyway it is easy to analyze kinematics of serial mechanisms
3 Kinematics of planar closed-loop link mechanisms
The simplestclosed-loop mechanism
Planar 4-bar link mechanism
θ1L3L 2L
)( YX出力
Input link
X
Y φ
0L
Follower link
Coupler link
ξ
η
ξ
η
J0
J1
J2
J3
P
O
Crank-rocker motion
Planar 4-bar link mechanismX
Y
X
Y
X
Y
X
Y
X
Y
X
Y
X
Y
Planar 4-bar link mechanismX
Y
X
Y
)( YXOutput coupler point
X
Y
O
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
(3)Control of Structural Flexibility and Its application
Output point
Motion control of a flexible link (coil spring)taking account of its reaction force
x z
y
Four legged walking machine with spring leg
(4)Estimation of Sound Radiation from Thin Platesubjected to Acoustic Excitation
Thin rectangular plate
Acoustic excitation
Transmitted sound power
Transmitted sound power should be estimated to designsilent structure
Pseudo random noise Impulsive signal
Transmitted sound power can be estimatedwith an adequate accuracy
Due to mode of vibration
Input sound pressure Input sound pressure
Transmitted sound powerTransmitted sound power
(5)Estimation of Sound Radiation from Frog-type Guiro
The calculated acceleration and sound pressureExpanded
(6)Development of Micro Ciliary Actuators in GroupBase plate
Micro machine driven by many artificial micro cilium actuators
Artificial cilium motion
Piezoelectric pipemorph actuator
Yy
2θ
L do
φdi
Elastic bar(Internal electrode)
Piezoelectric ceramics
External electrode
~
Piezoelectric Pipemorph actuator
Partial Ni plating with Watt solution bath
125μm
100μm
Fabrication of PZT film
~
Ni electrodes ~20kV
Internal electrodes
Vacuum desiccator Heater for soldering
Sn-Pb SolderNi plate
Several fabrication methods
Convey of a small sheet with micro cilium actuators in group
Microscope
Micro cilium actuators in group
A paper sheet 6mmtimes6mm
8μm
I hope you will visit my WEB-site
httpwwwdynamicsmeptitechacjpindexphp
if you are interested in other research works
Thank you
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Itrsquos easy to calculate output motion of themanipulator
2 Kinematics of planar serial link mechanisms
Forward kinematics
+++++minusminus
=
)cos()(sin)sin()(sin
10101000
10101000
θθθθθθθθθθθθ
LLLL
YX
P
P
By differentiating the displacement the output velocity can be calculated as
Linear relationship between output velocity and joint velocity
It can be deformed as
++++minus+minusminus
=
1
0
10110100
10110100
)cos()cos(cos)sin()sin(sin
θθ
θθθθθθθθθθ
LLLLLL
YX
P
P
Jacobian matrix J(θ) Joint velocityvector θ
Output velocityvector r
θθJr )(=
θθJθθJ
)()(
)]sin()()cos()([
)sincos()]cos()()sin()([
)cossin(
102
1010101
020000
102
1010101
020000
+=
++minus+++
minus+++++minus
+minus
=
θθθθθθθθ
θθθθθθθθθθθθ
θθθθ
L
LL
L
YX
P
P
Output acceleration can also be calculated as
I t is easy to analyze output motion with knowledge at high school
For general planar serial nR manipulatorhellip
+
+=
sum sum
sum summinus
= =
minus
= =
0
1
0 0
0
1
0 0
sin
cos
YL
XL
YX
n
i
i
kki
n
i
i
kki
P
P
θ
θ
minus
=
sum sumsum
sum sumsumminus
= ==
minus
= ==1
0 00
1
0 00
cos
sinn
i
i
kk
i
kki
n
i
i
kk
i
kki
P
P
L
L
YX
θθ
θθ
minus
+minus=
sum sum sum sumsum
sum sum sum sumsumminus
= = = ==
minus
= = = ==1
0 0 0 0
2
0
1
0 0 0 0
2
0
]sin)(cos[
]cos)(sin[n
i
i
k
i
k
i
kkkk
i
kki
n
i
i
k
i
k
i
kkkk
i
kki
P
P
L
L
YX
θθθθ
θθθθ
Itrsquos also easy to analyze
Inverse kinematics
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
To calculate joint input motion forthe specified output motion
Can θ0θ1 be calculated as the function of XPYP
It will be a little bit complicated
By using the prementioned equation for forward kinematics
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Let draw an auxiliary line
By defining angles αβ
βαθ minus=0
where
0
01tanXXYY
P
P
minusminus
= minusα
How to derive β X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
α
β
Planar serial 2R manipulator
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
β
Let consider PJ0J1
L
20
20 )()( YYXXL PP minus+minus=
By using side lengths L0L1 anddistance
0
21
20
2
2cos
LLLLL minus+
=β
We can apply cosine theorem
0
21
20
21
2cos
LLLLL minus+
= minusβTherefore
Planar serial 2R manipulator
0
21
20
21
0
01
0
2costan
LLLLL
XXYY
P
P minus+minus
minusminus
=
minus=
minusminus
βαθ
θ1 can then be calculated
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1 L
Let consider PJ0J1 and an angle γ
γ
θ1=πminusγ
10
221
201
2cos
LLLLL minus+
= minusγ
From cosine theorem
10
221
201
1
2cos
LLLLL minus+
minus=
minus=
minusπ
γπθ
Joint input velocity and acceleration can be derived as
γθ
γθ
βαθ
βαθ
minus=
minus=
minus=
minus=
1
1
0
0
γγ
sin10LLLL
=
γγγγγ 2
10
2
sincossin)(
LLLLLLL
minus+
=
LYYXXL PPPP
+
=
2
22 )()(L
LYYXXLYYYXXXL PPPPPPPPPP
+minus+++=
They can also be derived as
+minusminus+minusminus
++=
++++minus+minusminus
=
=minus
minus
P
P
P
P
YX
LLLLLL
LL
YX
LLLLLL
)sin(sin)cos(cos)sin()cos(
sin1
)cos()cos(cos)sin()sin(sin
)(
1010010100
101101
110
1
10110100
10110100
1
θθθθθθθθθθ
θ
θθθθθθθθθθ
rθJθ
++minus++minusminus++minus++minusminus
minus
times
+minusminus+minusminus
++=
minus= minus
1
0
1010110101000
1010110101000
1010010100
101101
110
1
)sin()()sin()(sin)cos()()cos()(cos
)sin(sin)cos(cos)sin()cos(
sin1
])([)(
θθ
θθθθθθθθθθθθθθθθθθθθ
θθθθθθθθθθ
θ
LLLLLL
YX
LLLLLL
LL
P
P
θθJrθJθ
Anyway it is easy to analyze kinematics of serial mechanisms
3 Kinematics of planar closed-loop link mechanisms
The simplestclosed-loop mechanism
Planar 4-bar link mechanism
θ1L3L 2L
)( YX出力
Input link
X
Y φ
0L
Follower link
Coupler link
ξ
η
ξ
η
J0
J1
J2
J3
P
O
Crank-rocker motion
Planar 4-bar link mechanismX
Y
X
Y
X
Y
X
Y
X
Y
X
Y
X
Y
Planar 4-bar link mechanismX
Y
X
Y
)( YXOutput coupler point
X
Y
O
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
x z
y
Four legged walking machine with spring leg
(4)Estimation of Sound Radiation from Thin Platesubjected to Acoustic Excitation
Thin rectangular plate
Acoustic excitation
Transmitted sound power
Transmitted sound power should be estimated to designsilent structure
Pseudo random noise Impulsive signal
Transmitted sound power can be estimatedwith an adequate accuracy
Due to mode of vibration
Input sound pressure Input sound pressure
Transmitted sound powerTransmitted sound power
(5)Estimation of Sound Radiation from Frog-type Guiro
The calculated acceleration and sound pressureExpanded
(6)Development of Micro Ciliary Actuators in GroupBase plate
Micro machine driven by many artificial micro cilium actuators
Artificial cilium motion
Piezoelectric pipemorph actuator
Yy
2θ
L do
φdi
Elastic bar(Internal electrode)
Piezoelectric ceramics
External electrode
~
Piezoelectric Pipemorph actuator
Partial Ni plating with Watt solution bath
125μm
100μm
Fabrication of PZT film
~
Ni electrodes ~20kV
Internal electrodes
Vacuum desiccator Heater for soldering
Sn-Pb SolderNi plate
Several fabrication methods
Convey of a small sheet with micro cilium actuators in group
Microscope
Micro cilium actuators in group
A paper sheet 6mmtimes6mm
8μm
I hope you will visit my WEB-site
httpwwwdynamicsmeptitechacjpindexphp
if you are interested in other research works
Thank you
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Itrsquos easy to calculate output motion of themanipulator
2 Kinematics of planar serial link mechanisms
Forward kinematics
+++++minusminus
=
)cos()(sin)sin()(sin
10101000
10101000
θθθθθθθθθθθθ
LLLL
YX
P
P
By differentiating the displacement the output velocity can be calculated as
Linear relationship between output velocity and joint velocity
It can be deformed as
++++minus+minusminus
=
1
0
10110100
10110100
)cos()cos(cos)sin()sin(sin
θθ
θθθθθθθθθθ
LLLLLL
YX
P
P
Jacobian matrix J(θ) Joint velocityvector θ
Output velocityvector r
θθJr )(=
θθJθθJ
)()(
)]sin()()cos()([
)sincos()]cos()()sin()([
)cossin(
102
1010101
020000
102
1010101
020000
+=
++minus+++
minus+++++minus
+minus
=
θθθθθθθθ
θθθθθθθθθθθθ
θθθθ
L
LL
L
YX
P
P
Output acceleration can also be calculated as
I t is easy to analyze output motion with knowledge at high school
For general planar serial nR manipulatorhellip
+
+=
sum sum
sum summinus
= =
minus
= =
0
1
0 0
0
1
0 0
sin
cos
YL
XL
YX
n
i
i
kki
n
i
i
kki
P
P
θ
θ
minus
=
sum sumsum
sum sumsumminus
= ==
minus
= ==1
0 00
1
0 00
cos
sinn
i
i
kk
i
kki
n
i
i
kk
i
kki
P
P
L
L
YX
θθ
θθ
minus
+minus=
sum sum sum sumsum
sum sum sum sumsumminus
= = = ==
minus
= = = ==1
0 0 0 0
2
0
1
0 0 0 0
2
0
]sin)(cos[
]cos)(sin[n
i
i
k
i
k
i
kkkk
i
kki
n
i
i
k
i
k
i
kkkk
i
kki
P
P
L
L
YX
θθθθ
θθθθ
Itrsquos also easy to analyze
Inverse kinematics
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
To calculate joint input motion forthe specified output motion
Can θ0θ1 be calculated as the function of XPYP
It will be a little bit complicated
By using the prementioned equation for forward kinematics
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Let draw an auxiliary line
By defining angles αβ
βαθ minus=0
where
0
01tanXXYY
P
P
minusminus
= minusα
How to derive β X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
α
β
Planar serial 2R manipulator
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
β
Let consider PJ0J1
L
20
20 )()( YYXXL PP minus+minus=
By using side lengths L0L1 anddistance
0
21
20
2
2cos
LLLLL minus+
=β
We can apply cosine theorem
0
21
20
21
2cos
LLLLL minus+
= minusβTherefore
Planar serial 2R manipulator
0
21
20
21
0
01
0
2costan
LLLLL
XXYY
P
P minus+minus
minusminus
=
minus=
minusminus
βαθ
θ1 can then be calculated
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1 L
Let consider PJ0J1 and an angle γ
γ
θ1=πminusγ
10
221
201
2cos
LLLLL minus+
= minusγ
From cosine theorem
10
221
201
1
2cos
LLLLL minus+
minus=
minus=
minusπ
γπθ
Joint input velocity and acceleration can be derived as
γθ
γθ
βαθ
βαθ
minus=
minus=
minus=
minus=
1
1
0
0
γγ
sin10LLLL
=
γγγγγ 2
10
2
sincossin)(
LLLLLLL
minus+
=
LYYXXL PPPP
+
=
2
22 )()(L
LYYXXLYYYXXXL PPPPPPPPPP
+minus+++=
They can also be derived as
+minusminus+minusminus
++=
++++minus+minusminus
=
=minus
minus
P
P
P
P
YX
LLLLLL
LL
YX
LLLLLL
)sin(sin)cos(cos)sin()cos(
sin1
)cos()cos(cos)sin()sin(sin
)(
1010010100
101101
110
1
10110100
10110100
1
θθθθθθθθθθ
θ
θθθθθθθθθθ
rθJθ
++minus++minusminus++minus++minusminus
minus
times
+minusminus+minusminus
++=
minus= minus
1
0
1010110101000
1010110101000
1010010100
101101
110
1
)sin()()sin()(sin)cos()()cos()(cos
)sin(sin)cos(cos)sin()cos(
sin1
])([)(
θθ
θθθθθθθθθθθθθθθθθθθθ
θθθθθθθθθθ
θ
LLLLLL
YX
LLLLLL
LL
P
P
θθJrθJθ
Anyway it is easy to analyze kinematics of serial mechanisms
3 Kinematics of planar closed-loop link mechanisms
The simplestclosed-loop mechanism
Planar 4-bar link mechanism
θ1L3L 2L
)( YX出力
Input link
X
Y φ
0L
Follower link
Coupler link
ξ
η
ξ
η
J0
J1
J2
J3
P
O
Crank-rocker motion
Planar 4-bar link mechanismX
Y
X
Y
X
Y
X
Y
X
Y
X
Y
X
Y
Planar 4-bar link mechanismX
Y
X
Y
)( YXOutput coupler point
X
Y
O
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
(4)Estimation of Sound Radiation from Thin Platesubjected to Acoustic Excitation
Thin rectangular plate
Acoustic excitation
Transmitted sound power
Transmitted sound power should be estimated to designsilent structure
Pseudo random noise Impulsive signal
Transmitted sound power can be estimatedwith an adequate accuracy
Due to mode of vibration
Input sound pressure Input sound pressure
Transmitted sound powerTransmitted sound power
(5)Estimation of Sound Radiation from Frog-type Guiro
The calculated acceleration and sound pressureExpanded
(6)Development of Micro Ciliary Actuators in GroupBase plate
Micro machine driven by many artificial micro cilium actuators
Artificial cilium motion
Piezoelectric pipemorph actuator
Yy
2θ
L do
φdi
Elastic bar(Internal electrode)
Piezoelectric ceramics
External electrode
~
Piezoelectric Pipemorph actuator
Partial Ni plating with Watt solution bath
125μm
100μm
Fabrication of PZT film
~
Ni electrodes ~20kV
Internal electrodes
Vacuum desiccator Heater for soldering
Sn-Pb SolderNi plate
Several fabrication methods
Convey of a small sheet with micro cilium actuators in group
Microscope
Micro cilium actuators in group
A paper sheet 6mmtimes6mm
8μm
I hope you will visit my WEB-site
httpwwwdynamicsmeptitechacjpindexphp
if you are interested in other research works
Thank you
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Itrsquos easy to calculate output motion of themanipulator
2 Kinematics of planar serial link mechanisms
Forward kinematics
+++++minusminus
=
)cos()(sin)sin()(sin
10101000
10101000
θθθθθθθθθθθθ
LLLL
YX
P
P
By differentiating the displacement the output velocity can be calculated as
Linear relationship between output velocity and joint velocity
It can be deformed as
++++minus+minusminus
=
1
0
10110100
10110100
)cos()cos(cos)sin()sin(sin
θθ
θθθθθθθθθθ
LLLLLL
YX
P
P
Jacobian matrix J(θ) Joint velocityvector θ
Output velocityvector r
θθJr )(=
θθJθθJ
)()(
)]sin()()cos()([
)sincos()]cos()()sin()([
)cossin(
102
1010101
020000
102
1010101
020000
+=
++minus+++
minus+++++minus
+minus
=
θθθθθθθθ
θθθθθθθθθθθθ
θθθθ
L
LL
L
YX
P
P
Output acceleration can also be calculated as
I t is easy to analyze output motion with knowledge at high school
For general planar serial nR manipulatorhellip
+
+=
sum sum
sum summinus
= =
minus
= =
0
1
0 0
0
1
0 0
sin
cos
YL
XL
YX
n
i
i
kki
n
i
i
kki
P
P
θ
θ
minus
=
sum sumsum
sum sumsumminus
= ==
minus
= ==1
0 00
1
0 00
cos
sinn
i
i
kk
i
kki
n
i
i
kk
i
kki
P
P
L
L
YX
θθ
θθ
minus
+minus=
sum sum sum sumsum
sum sum sum sumsumminus
= = = ==
minus
= = = ==1
0 0 0 0
2
0
1
0 0 0 0
2
0
]sin)(cos[
]cos)(sin[n
i
i
k
i
k
i
kkkk
i
kki
n
i
i
k
i
k
i
kkkk
i
kki
P
P
L
L
YX
θθθθ
θθθθ
Itrsquos also easy to analyze
Inverse kinematics
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
To calculate joint input motion forthe specified output motion
Can θ0θ1 be calculated as the function of XPYP
It will be a little bit complicated
By using the prementioned equation for forward kinematics
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Let draw an auxiliary line
By defining angles αβ
βαθ minus=0
where
0
01tanXXYY
P
P
minusminus
= minusα
How to derive β X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
α
β
Planar serial 2R manipulator
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
β
Let consider PJ0J1
L
20
20 )()( YYXXL PP minus+minus=
By using side lengths L0L1 anddistance
0
21
20
2
2cos
LLLLL minus+
=β
We can apply cosine theorem
0
21
20
21
2cos
LLLLL minus+
= minusβTherefore
Planar serial 2R manipulator
0
21
20
21
0
01
0
2costan
LLLLL
XXYY
P
P minus+minus
minusminus
=
minus=
minusminus
βαθ
θ1 can then be calculated
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1 L
Let consider PJ0J1 and an angle γ
γ
θ1=πminusγ
10
221
201
2cos
LLLLL minus+
= minusγ
From cosine theorem
10
221
201
1
2cos
LLLLL minus+
minus=
minus=
minusπ
γπθ
Joint input velocity and acceleration can be derived as
γθ
γθ
βαθ
βαθ
minus=
minus=
minus=
minus=
1
1
0
0
γγ
sin10LLLL
=
γγγγγ 2
10
2
sincossin)(
LLLLLLL
minus+
=
LYYXXL PPPP
+
=
2
22 )()(L
LYYXXLYYYXXXL PPPPPPPPPP
+minus+++=
They can also be derived as
+minusminus+minusminus
++=
++++minus+minusminus
=
=minus
minus
P
P
P
P
YX
LLLLLL
LL
YX
LLLLLL
)sin(sin)cos(cos)sin()cos(
sin1
)cos()cos(cos)sin()sin(sin
)(
1010010100
101101
110
1
10110100
10110100
1
θθθθθθθθθθ
θ
θθθθθθθθθθ
rθJθ
++minus++minusminus++minus++minusminus
minus
times
+minusminus+minusminus
++=
minus= minus
1
0
1010110101000
1010110101000
1010010100
101101
110
1
)sin()()sin()(sin)cos()()cos()(cos
)sin(sin)cos(cos)sin()cos(
sin1
])([)(
θθ
θθθθθθθθθθθθθθθθθθθθ
θθθθθθθθθθ
θ
LLLLLL
YX
LLLLLL
LL
P
P
θθJrθJθ
Anyway it is easy to analyze kinematics of serial mechanisms
3 Kinematics of planar closed-loop link mechanisms
The simplestclosed-loop mechanism
Planar 4-bar link mechanism
θ1L3L 2L
)( YX出力
Input link
X
Y φ
0L
Follower link
Coupler link
ξ
η
ξ
η
J0
J1
J2
J3
P
O
Crank-rocker motion
Planar 4-bar link mechanismX
Y
X
Y
X
Y
X
Y
X
Y
X
Y
X
Y
Planar 4-bar link mechanismX
Y
X
Y
)( YXOutput coupler point
X
Y
O
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
Pseudo random noise Impulsive signal
Transmitted sound power can be estimatedwith an adequate accuracy
Due to mode of vibration
Input sound pressure Input sound pressure
Transmitted sound powerTransmitted sound power
(5)Estimation of Sound Radiation from Frog-type Guiro
The calculated acceleration and sound pressureExpanded
(6)Development of Micro Ciliary Actuators in GroupBase plate
Micro machine driven by many artificial micro cilium actuators
Artificial cilium motion
Piezoelectric pipemorph actuator
Yy
2θ
L do
φdi
Elastic bar(Internal electrode)
Piezoelectric ceramics
External electrode
~
Piezoelectric Pipemorph actuator
Partial Ni plating with Watt solution bath
125μm
100μm
Fabrication of PZT film
~
Ni electrodes ~20kV
Internal electrodes
Vacuum desiccator Heater for soldering
Sn-Pb SolderNi plate
Several fabrication methods
Convey of a small sheet with micro cilium actuators in group
Microscope
Micro cilium actuators in group
A paper sheet 6mmtimes6mm
8μm
I hope you will visit my WEB-site
httpwwwdynamicsmeptitechacjpindexphp
if you are interested in other research works
Thank you
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Itrsquos easy to calculate output motion of themanipulator
2 Kinematics of planar serial link mechanisms
Forward kinematics
+++++minusminus
=
)cos()(sin)sin()(sin
10101000
10101000
θθθθθθθθθθθθ
LLLL
YX
P
P
By differentiating the displacement the output velocity can be calculated as
Linear relationship between output velocity and joint velocity
It can be deformed as
++++minus+minusminus
=
1
0
10110100
10110100
)cos()cos(cos)sin()sin(sin
θθ
θθθθθθθθθθ
LLLLLL
YX
P
P
Jacobian matrix J(θ) Joint velocityvector θ
Output velocityvector r
θθJr )(=
θθJθθJ
)()(
)]sin()()cos()([
)sincos()]cos()()sin()([
)cossin(
102
1010101
020000
102
1010101
020000
+=
++minus+++
minus+++++minus
+minus
=
θθθθθθθθ
θθθθθθθθθθθθ
θθθθ
L
LL
L
YX
P
P
Output acceleration can also be calculated as
I t is easy to analyze output motion with knowledge at high school
For general planar serial nR manipulatorhellip
+
+=
sum sum
sum summinus
= =
minus
= =
0
1
0 0
0
1
0 0
sin
cos
YL
XL
YX
n
i
i
kki
n
i
i
kki
P
P
θ
θ
minus
=
sum sumsum
sum sumsumminus
= ==
minus
= ==1
0 00
1
0 00
cos
sinn
i
i
kk
i
kki
n
i
i
kk
i
kki
P
P
L
L
YX
θθ
θθ
minus
+minus=
sum sum sum sumsum
sum sum sum sumsumminus
= = = ==
minus
= = = ==1
0 0 0 0
2
0
1
0 0 0 0
2
0
]sin)(cos[
]cos)(sin[n
i
i
k
i
k
i
kkkk
i
kki
n
i
i
k
i
k
i
kkkk
i
kki
P
P
L
L
YX
θθθθ
θθθθ
Itrsquos also easy to analyze
Inverse kinematics
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
To calculate joint input motion forthe specified output motion
Can θ0θ1 be calculated as the function of XPYP
It will be a little bit complicated
By using the prementioned equation for forward kinematics
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Let draw an auxiliary line
By defining angles αβ
βαθ minus=0
where
0
01tanXXYY
P
P
minusminus
= minusα
How to derive β X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
α
β
Planar serial 2R manipulator
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
β
Let consider PJ0J1
L
20
20 )()( YYXXL PP minus+minus=
By using side lengths L0L1 anddistance
0
21
20
2
2cos
LLLLL minus+
=β
We can apply cosine theorem
0
21
20
21
2cos
LLLLL minus+
= minusβTherefore
Planar serial 2R manipulator
0
21
20
21
0
01
0
2costan
LLLLL
XXYY
P
P minus+minus
minusminus
=
minus=
minusminus
βαθ
θ1 can then be calculated
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1 L
Let consider PJ0J1 and an angle γ
γ
θ1=πminusγ
10
221
201
2cos
LLLLL minus+
= minusγ
From cosine theorem
10
221
201
1
2cos
LLLLL minus+
minus=
minus=
minusπ
γπθ
Joint input velocity and acceleration can be derived as
γθ
γθ
βαθ
βαθ
minus=
minus=
minus=
minus=
1
1
0
0
γγ
sin10LLLL
=
γγγγγ 2
10
2
sincossin)(
LLLLLLL
minus+
=
LYYXXL PPPP
+
=
2
22 )()(L
LYYXXLYYYXXXL PPPPPPPPPP
+minus+++=
They can also be derived as
+minusminus+minusminus
++=
++++minus+minusminus
=
=minus
minus
P
P
P
P
YX
LLLLLL
LL
YX
LLLLLL
)sin(sin)cos(cos)sin()cos(
sin1
)cos()cos(cos)sin()sin(sin
)(
1010010100
101101
110
1
10110100
10110100
1
θθθθθθθθθθ
θ
θθθθθθθθθθ
rθJθ
++minus++minusminus++minus++minusminus
minus
times
+minusminus+minusminus
++=
minus= minus
1
0
1010110101000
1010110101000
1010010100
101101
110
1
)sin()()sin()(sin)cos()()cos()(cos
)sin(sin)cos(cos)sin()cos(
sin1
])([)(
θθ
θθθθθθθθθθθθθθθθθθθθ
θθθθθθθθθθ
θ
LLLLLL
YX
LLLLLL
LL
P
P
θθJrθJθ
Anyway it is easy to analyze kinematics of serial mechanisms
3 Kinematics of planar closed-loop link mechanisms
The simplestclosed-loop mechanism
Planar 4-bar link mechanism
θ1L3L 2L
)( YX出力
Input link
X
Y φ
0L
Follower link
Coupler link
ξ
η
ξ
η
J0
J1
J2
J3
P
O
Crank-rocker motion
Planar 4-bar link mechanismX
Y
X
Y
X
Y
X
Y
X
Y
X
Y
X
Y
Planar 4-bar link mechanismX
Y
X
Y
)( YXOutput coupler point
X
Y
O
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
(5)Estimation of Sound Radiation from Frog-type Guiro
The calculated acceleration and sound pressureExpanded
(6)Development of Micro Ciliary Actuators in GroupBase plate
Micro machine driven by many artificial micro cilium actuators
Artificial cilium motion
Piezoelectric pipemorph actuator
Yy
2θ
L do
φdi
Elastic bar(Internal electrode)
Piezoelectric ceramics
External electrode
~
Piezoelectric Pipemorph actuator
Partial Ni plating with Watt solution bath
125μm
100μm
Fabrication of PZT film
~
Ni electrodes ~20kV
Internal electrodes
Vacuum desiccator Heater for soldering
Sn-Pb SolderNi plate
Several fabrication methods
Convey of a small sheet with micro cilium actuators in group
Microscope
Micro cilium actuators in group
A paper sheet 6mmtimes6mm
8μm
I hope you will visit my WEB-site
httpwwwdynamicsmeptitechacjpindexphp
if you are interested in other research works
Thank you
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Itrsquos easy to calculate output motion of themanipulator
2 Kinematics of planar serial link mechanisms
Forward kinematics
+++++minusminus
=
)cos()(sin)sin()(sin
10101000
10101000
θθθθθθθθθθθθ
LLLL
YX
P
P
By differentiating the displacement the output velocity can be calculated as
Linear relationship between output velocity and joint velocity
It can be deformed as
++++minus+minusminus
=
1
0
10110100
10110100
)cos()cos(cos)sin()sin(sin
θθ
θθθθθθθθθθ
LLLLLL
YX
P
P
Jacobian matrix J(θ) Joint velocityvector θ
Output velocityvector r
θθJr )(=
θθJθθJ
)()(
)]sin()()cos()([
)sincos()]cos()()sin()([
)cossin(
102
1010101
020000
102
1010101
020000
+=
++minus+++
minus+++++minus
+minus
=
θθθθθθθθ
θθθθθθθθθθθθ
θθθθ
L
LL
L
YX
P
P
Output acceleration can also be calculated as
I t is easy to analyze output motion with knowledge at high school
For general planar serial nR manipulatorhellip
+
+=
sum sum
sum summinus
= =
minus
= =
0
1
0 0
0
1
0 0
sin
cos
YL
XL
YX
n
i
i
kki
n
i
i
kki
P
P
θ
θ
minus
=
sum sumsum
sum sumsumminus
= ==
minus
= ==1
0 00
1
0 00
cos
sinn
i
i
kk
i
kki
n
i
i
kk
i
kki
P
P
L
L
YX
θθ
θθ
minus
+minus=
sum sum sum sumsum
sum sum sum sumsumminus
= = = ==
minus
= = = ==1
0 0 0 0
2
0
1
0 0 0 0
2
0
]sin)(cos[
]cos)(sin[n
i
i
k
i
k
i
kkkk
i
kki
n
i
i
k
i
k
i
kkkk
i
kki
P
P
L
L
YX
θθθθ
θθθθ
Itrsquos also easy to analyze
Inverse kinematics
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
To calculate joint input motion forthe specified output motion
Can θ0θ1 be calculated as the function of XPYP
It will be a little bit complicated
By using the prementioned equation for forward kinematics
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Let draw an auxiliary line
By defining angles αβ
βαθ minus=0
where
0
01tanXXYY
P
P
minusminus
= minusα
How to derive β X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
α
β
Planar serial 2R manipulator
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
β
Let consider PJ0J1
L
20
20 )()( YYXXL PP minus+minus=
By using side lengths L0L1 anddistance
0
21
20
2
2cos
LLLLL minus+
=β
We can apply cosine theorem
0
21
20
21
2cos
LLLLL minus+
= minusβTherefore
Planar serial 2R manipulator
0
21
20
21
0
01
0
2costan
LLLLL
XXYY
P
P minus+minus
minusminus
=
minus=
minusminus
βαθ
θ1 can then be calculated
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1 L
Let consider PJ0J1 and an angle γ
γ
θ1=πminusγ
10
221
201
2cos
LLLLL minus+
= minusγ
From cosine theorem
10
221
201
1
2cos
LLLLL minus+
minus=
minus=
minusπ
γπθ
Joint input velocity and acceleration can be derived as
γθ
γθ
βαθ
βαθ
minus=
minus=
minus=
minus=
1
1
0
0
γγ
sin10LLLL
=
γγγγγ 2
10
2
sincossin)(
LLLLLLL
minus+
=
LYYXXL PPPP
+
=
2
22 )()(L
LYYXXLYYYXXXL PPPPPPPPPP
+minus+++=
They can also be derived as
+minusminus+minusminus
++=
++++minus+minusminus
=
=minus
minus
P
P
P
P
YX
LLLLLL
LL
YX
LLLLLL
)sin(sin)cos(cos)sin()cos(
sin1
)cos()cos(cos)sin()sin(sin
)(
1010010100
101101
110
1
10110100
10110100
1
θθθθθθθθθθ
θ
θθθθθθθθθθ
rθJθ
++minus++minusminus++minus++minusminus
minus
times
+minusminus+minusminus
++=
minus= minus
1
0
1010110101000
1010110101000
1010010100
101101
110
1
)sin()()sin()(sin)cos()()cos()(cos
)sin(sin)cos(cos)sin()cos(
sin1
])([)(
θθ
θθθθθθθθθθθθθθθθθθθθ
θθθθθθθθθθ
θ
LLLLLL
YX
LLLLLL
LL
P
P
θθJrθJθ
Anyway it is easy to analyze kinematics of serial mechanisms
3 Kinematics of planar closed-loop link mechanisms
The simplestclosed-loop mechanism
Planar 4-bar link mechanism
θ1L3L 2L
)( YX出力
Input link
X
Y φ
0L
Follower link
Coupler link
ξ
η
ξ
η
J0
J1
J2
J3
P
O
Crank-rocker motion
Planar 4-bar link mechanismX
Y
X
Y
X
Y
X
Y
X
Y
X
Y
X
Y
Planar 4-bar link mechanismX
Y
X
Y
)( YXOutput coupler point
X
Y
O
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
(6)Development of Micro Ciliary Actuators in GroupBase plate
Micro machine driven by many artificial micro cilium actuators
Artificial cilium motion
Piezoelectric pipemorph actuator
Yy
2θ
L do
φdi
Elastic bar(Internal electrode)
Piezoelectric ceramics
External electrode
~
Piezoelectric Pipemorph actuator
Partial Ni plating with Watt solution bath
125μm
100μm
Fabrication of PZT film
~
Ni electrodes ~20kV
Internal electrodes
Vacuum desiccator Heater for soldering
Sn-Pb SolderNi plate
Several fabrication methods
Convey of a small sheet with micro cilium actuators in group
Microscope
Micro cilium actuators in group
A paper sheet 6mmtimes6mm
8μm
I hope you will visit my WEB-site
httpwwwdynamicsmeptitechacjpindexphp
if you are interested in other research works
Thank you
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Itrsquos easy to calculate output motion of themanipulator
2 Kinematics of planar serial link mechanisms
Forward kinematics
+++++minusminus
=
)cos()(sin)sin()(sin
10101000
10101000
θθθθθθθθθθθθ
LLLL
YX
P
P
By differentiating the displacement the output velocity can be calculated as
Linear relationship between output velocity and joint velocity
It can be deformed as
++++minus+minusminus
=
1
0
10110100
10110100
)cos()cos(cos)sin()sin(sin
θθ
θθθθθθθθθθ
LLLLLL
YX
P
P
Jacobian matrix J(θ) Joint velocityvector θ
Output velocityvector r
θθJr )(=
θθJθθJ
)()(
)]sin()()cos()([
)sincos()]cos()()sin()([
)cossin(
102
1010101
020000
102
1010101
020000
+=
++minus+++
minus+++++minus
+minus
=
θθθθθθθθ
θθθθθθθθθθθθ
θθθθ
L
LL
L
YX
P
P
Output acceleration can also be calculated as
I t is easy to analyze output motion with knowledge at high school
For general planar serial nR manipulatorhellip
+
+=
sum sum
sum summinus
= =
minus
= =
0
1
0 0
0
1
0 0
sin
cos
YL
XL
YX
n
i
i
kki
n
i
i
kki
P
P
θ
θ
minus
=
sum sumsum
sum sumsumminus
= ==
minus
= ==1
0 00
1
0 00
cos
sinn
i
i
kk
i
kki
n
i
i
kk
i
kki
P
P
L
L
YX
θθ
θθ
minus
+minus=
sum sum sum sumsum
sum sum sum sumsumminus
= = = ==
minus
= = = ==1
0 0 0 0
2
0
1
0 0 0 0
2
0
]sin)(cos[
]cos)(sin[n
i
i
k
i
k
i
kkkk
i
kki
n
i
i
k
i
k
i
kkkk
i
kki
P
P
L
L
YX
θθθθ
θθθθ
Itrsquos also easy to analyze
Inverse kinematics
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
To calculate joint input motion forthe specified output motion
Can θ0θ1 be calculated as the function of XPYP
It will be a little bit complicated
By using the prementioned equation for forward kinematics
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Let draw an auxiliary line
By defining angles αβ
βαθ minus=0
where
0
01tanXXYY
P
P
minusminus
= minusα
How to derive β X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
α
β
Planar serial 2R manipulator
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
β
Let consider PJ0J1
L
20
20 )()( YYXXL PP minus+minus=
By using side lengths L0L1 anddistance
0
21
20
2
2cos
LLLLL minus+
=β
We can apply cosine theorem
0
21
20
21
2cos
LLLLL minus+
= minusβTherefore
Planar serial 2R manipulator
0
21
20
21
0
01
0
2costan
LLLLL
XXYY
P
P minus+minus
minusminus
=
minus=
minusminus
βαθ
θ1 can then be calculated
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1 L
Let consider PJ0J1 and an angle γ
γ
θ1=πminusγ
10
221
201
2cos
LLLLL minus+
= minusγ
From cosine theorem
10
221
201
1
2cos
LLLLL minus+
minus=
minus=
minusπ
γπθ
Joint input velocity and acceleration can be derived as
γθ
γθ
βαθ
βαθ
minus=
minus=
minus=
minus=
1
1
0
0
γγ
sin10LLLL
=
γγγγγ 2
10
2
sincossin)(
LLLLLLL
minus+
=
LYYXXL PPPP
+
=
2
22 )()(L
LYYXXLYYYXXXL PPPPPPPPPP
+minus+++=
They can also be derived as
+minusminus+minusminus
++=
++++minus+minusminus
=
=minus
minus
P
P
P
P
YX
LLLLLL
LL
YX
LLLLLL
)sin(sin)cos(cos)sin()cos(
sin1
)cos()cos(cos)sin()sin(sin
)(
1010010100
101101
110
1
10110100
10110100
1
θθθθθθθθθθ
θ
θθθθθθθθθθ
rθJθ
++minus++minusminus++minus++minusminus
minus
times
+minusminus+minusminus
++=
minus= minus
1
0
1010110101000
1010110101000
1010010100
101101
110
1
)sin()()sin()(sin)cos()()cos()(cos
)sin(sin)cos(cos)sin()cos(
sin1
])([)(
θθ
θθθθθθθθθθθθθθθθθθθθ
θθθθθθθθθθ
θ
LLLLLL
YX
LLLLLL
LL
P
P
θθJrθJθ
Anyway it is easy to analyze kinematics of serial mechanisms
3 Kinematics of planar closed-loop link mechanisms
The simplestclosed-loop mechanism
Planar 4-bar link mechanism
θ1L3L 2L
)( YX出力
Input link
X
Y φ
0L
Follower link
Coupler link
ξ
η
ξ
η
J0
J1
J2
J3
P
O
Crank-rocker motion
Planar 4-bar link mechanismX
Y
X
Y
X
Y
X
Y
X
Y
X
Y
X
Y
Planar 4-bar link mechanismX
Y
X
Y
)( YXOutput coupler point
X
Y
O
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
Partial Ni plating with Watt solution bath
125μm
100μm
Fabrication of PZT film
~
Ni electrodes ~20kV
Internal electrodes
Vacuum desiccator Heater for soldering
Sn-Pb SolderNi plate
Several fabrication methods
Convey of a small sheet with micro cilium actuators in group
Microscope
Micro cilium actuators in group
A paper sheet 6mmtimes6mm
8μm
I hope you will visit my WEB-site
httpwwwdynamicsmeptitechacjpindexphp
if you are interested in other research works
Thank you
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Itrsquos easy to calculate output motion of themanipulator
2 Kinematics of planar serial link mechanisms
Forward kinematics
+++++minusminus
=
)cos()(sin)sin()(sin
10101000
10101000
θθθθθθθθθθθθ
LLLL
YX
P
P
By differentiating the displacement the output velocity can be calculated as
Linear relationship between output velocity and joint velocity
It can be deformed as
++++minus+minusminus
=
1
0
10110100
10110100
)cos()cos(cos)sin()sin(sin
θθ
θθθθθθθθθθ
LLLLLL
YX
P
P
Jacobian matrix J(θ) Joint velocityvector θ
Output velocityvector r
θθJr )(=
θθJθθJ
)()(
)]sin()()cos()([
)sincos()]cos()()sin()([
)cossin(
102
1010101
020000
102
1010101
020000
+=
++minus+++
minus+++++minus
+minus
=
θθθθθθθθ
θθθθθθθθθθθθ
θθθθ
L
LL
L
YX
P
P
Output acceleration can also be calculated as
I t is easy to analyze output motion with knowledge at high school
For general planar serial nR manipulatorhellip
+
+=
sum sum
sum summinus
= =
minus
= =
0
1
0 0
0
1
0 0
sin
cos
YL
XL
YX
n
i
i
kki
n
i
i
kki
P
P
θ
θ
minus
=
sum sumsum
sum sumsumminus
= ==
minus
= ==1
0 00
1
0 00
cos
sinn
i
i
kk
i
kki
n
i
i
kk
i
kki
P
P
L
L
YX
θθ
θθ
minus
+minus=
sum sum sum sumsum
sum sum sum sumsumminus
= = = ==
minus
= = = ==1
0 0 0 0
2
0
1
0 0 0 0
2
0
]sin)(cos[
]cos)(sin[n
i
i
k
i
k
i
kkkk
i
kki
n
i
i
k
i
k
i
kkkk
i
kki
P
P
L
L
YX
θθθθ
θθθθ
Itrsquos also easy to analyze
Inverse kinematics
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
To calculate joint input motion forthe specified output motion
Can θ0θ1 be calculated as the function of XPYP
It will be a little bit complicated
By using the prementioned equation for forward kinematics
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Let draw an auxiliary line
By defining angles αβ
βαθ minus=0
where
0
01tanXXYY
P
P
minusminus
= minusα
How to derive β X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
α
β
Planar serial 2R manipulator
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
β
Let consider PJ0J1
L
20
20 )()( YYXXL PP minus+minus=
By using side lengths L0L1 anddistance
0
21
20
2
2cos
LLLLL minus+
=β
We can apply cosine theorem
0
21
20
21
2cos
LLLLL minus+
= minusβTherefore
Planar serial 2R manipulator
0
21
20
21
0
01
0
2costan
LLLLL
XXYY
P
P minus+minus
minusminus
=
minus=
minusminus
βαθ
θ1 can then be calculated
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1 L
Let consider PJ0J1 and an angle γ
γ
θ1=πminusγ
10
221
201
2cos
LLLLL minus+
= minusγ
From cosine theorem
10
221
201
1
2cos
LLLLL minus+
minus=
minus=
minusπ
γπθ
Joint input velocity and acceleration can be derived as
γθ
γθ
βαθ
βαθ
minus=
minus=
minus=
minus=
1
1
0
0
γγ
sin10LLLL
=
γγγγγ 2
10
2
sincossin)(
LLLLLLL
minus+
=
LYYXXL PPPP
+
=
2
22 )()(L
LYYXXLYYYXXXL PPPPPPPPPP
+minus+++=
They can also be derived as
+minusminus+minusminus
++=
++++minus+minusminus
=
=minus
minus
P
P
P
P
YX
LLLLLL
LL
YX
LLLLLL
)sin(sin)cos(cos)sin()cos(
sin1
)cos()cos(cos)sin()sin(sin
)(
1010010100
101101
110
1
10110100
10110100
1
θθθθθθθθθθ
θ
θθθθθθθθθθ
rθJθ
++minus++minusminus++minus++minusminus
minus
times
+minusminus+minusminus
++=
minus= minus
1
0
1010110101000
1010110101000
1010010100
101101
110
1
)sin()()sin()(sin)cos()()cos()(cos
)sin(sin)cos(cos)sin()cos(
sin1
])([)(
θθ
θθθθθθθθθθθθθθθθθθθθ
θθθθθθθθθθ
θ
LLLLLL
YX
LLLLLL
LL
P
P
θθJrθJθ
Anyway it is easy to analyze kinematics of serial mechanisms
3 Kinematics of planar closed-loop link mechanisms
The simplestclosed-loop mechanism
Planar 4-bar link mechanism
θ1L3L 2L
)( YX出力
Input link
X
Y φ
0L
Follower link
Coupler link
ξ
η
ξ
η
J0
J1
J2
J3
P
O
Crank-rocker motion
Planar 4-bar link mechanismX
Y
X
Y
X
Y
X
Y
X
Y
X
Y
X
Y
Planar 4-bar link mechanismX
Y
X
Y
)( YXOutput coupler point
X
Y
O
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
Convey of a small sheet with micro cilium actuators in group
Microscope
Micro cilium actuators in group
A paper sheet 6mmtimes6mm
8μm
I hope you will visit my WEB-site
httpwwwdynamicsmeptitechacjpindexphp
if you are interested in other research works
Thank you
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Itrsquos easy to calculate output motion of themanipulator
2 Kinematics of planar serial link mechanisms
Forward kinematics
+++++minusminus
=
)cos()(sin)sin()(sin
10101000
10101000
θθθθθθθθθθθθ
LLLL
YX
P
P
By differentiating the displacement the output velocity can be calculated as
Linear relationship between output velocity and joint velocity
It can be deformed as
++++minus+minusminus
=
1
0
10110100
10110100
)cos()cos(cos)sin()sin(sin
θθ
θθθθθθθθθθ
LLLLLL
YX
P
P
Jacobian matrix J(θ) Joint velocityvector θ
Output velocityvector r
θθJr )(=
θθJθθJ
)()(
)]sin()()cos()([
)sincos()]cos()()sin()([
)cossin(
102
1010101
020000
102
1010101
020000
+=
++minus+++
minus+++++minus
+minus
=
θθθθθθθθ
θθθθθθθθθθθθ
θθθθ
L
LL
L
YX
P
P
Output acceleration can also be calculated as
I t is easy to analyze output motion with knowledge at high school
For general planar serial nR manipulatorhellip
+
+=
sum sum
sum summinus
= =
minus
= =
0
1
0 0
0
1
0 0
sin
cos
YL
XL
YX
n
i
i
kki
n
i
i
kki
P
P
θ
θ
minus
=
sum sumsum
sum sumsumminus
= ==
minus
= ==1
0 00
1
0 00
cos
sinn
i
i
kk
i
kki
n
i
i
kk
i
kki
P
P
L
L
YX
θθ
θθ
minus
+minus=
sum sum sum sumsum
sum sum sum sumsumminus
= = = ==
minus
= = = ==1
0 0 0 0
2
0
1
0 0 0 0
2
0
]sin)(cos[
]cos)(sin[n
i
i
k
i
k
i
kkkk
i
kki
n
i
i
k
i
k
i
kkkk
i
kki
P
P
L
L
YX
θθθθ
θθθθ
Itrsquos also easy to analyze
Inverse kinematics
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
To calculate joint input motion forthe specified output motion
Can θ0θ1 be calculated as the function of XPYP
It will be a little bit complicated
By using the prementioned equation for forward kinematics
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Let draw an auxiliary line
By defining angles αβ
βαθ minus=0
where
0
01tanXXYY
P
P
minusminus
= minusα
How to derive β X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
α
β
Planar serial 2R manipulator
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
β
Let consider PJ0J1
L
20
20 )()( YYXXL PP minus+minus=
By using side lengths L0L1 anddistance
0
21
20
2
2cos
LLLLL minus+
=β
We can apply cosine theorem
0
21
20
21
2cos
LLLLL minus+
= minusβTherefore
Planar serial 2R manipulator
0
21
20
21
0
01
0
2costan
LLLLL
XXYY
P
P minus+minus
minusminus
=
minus=
minusminus
βαθ
θ1 can then be calculated
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1 L
Let consider PJ0J1 and an angle γ
γ
θ1=πminusγ
10
221
201
2cos
LLLLL minus+
= minusγ
From cosine theorem
10
221
201
1
2cos
LLLLL minus+
minus=
minus=
minusπ
γπθ
Joint input velocity and acceleration can be derived as
γθ
γθ
βαθ
βαθ
minus=
minus=
minus=
minus=
1
1
0
0
γγ
sin10LLLL
=
γγγγγ 2
10
2
sincossin)(
LLLLLLL
minus+
=
LYYXXL PPPP
+
=
2
22 )()(L
LYYXXLYYYXXXL PPPPPPPPPP
+minus+++=
They can also be derived as
+minusminus+minusminus
++=
++++minus+minusminus
=
=minus
minus
P
P
P
P
YX
LLLLLL
LL
YX
LLLLLL
)sin(sin)cos(cos)sin()cos(
sin1
)cos()cos(cos)sin()sin(sin
)(
1010010100
101101
110
1
10110100
10110100
1
θθθθθθθθθθ
θ
θθθθθθθθθθ
rθJθ
++minus++minusminus++minus++minusminus
minus
times
+minusminus+minusminus
++=
minus= minus
1
0
1010110101000
1010110101000
1010010100
101101
110
1
)sin()()sin()(sin)cos()()cos()(cos
)sin(sin)cos(cos)sin()cos(
sin1
])([)(
θθ
θθθθθθθθθθθθθθθθθθθθ
θθθθθθθθθθ
θ
LLLLLL
YX
LLLLLL
LL
P
P
θθJrθJθ
Anyway it is easy to analyze kinematics of serial mechanisms
3 Kinematics of planar closed-loop link mechanisms
The simplestclosed-loop mechanism
Planar 4-bar link mechanism
θ1L3L 2L
)( YX出力
Input link
X
Y φ
0L
Follower link
Coupler link
ξ
η
ξ
η
J0
J1
J2
J3
P
O
Crank-rocker motion
Planar 4-bar link mechanismX
Y
X
Y
X
Y
X
Y
X
Y
X
Y
X
Y
Planar 4-bar link mechanismX
Y
X
Y
)( YXOutput coupler point
X
Y
O
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
I hope you will visit my WEB-site
httpwwwdynamicsmeptitechacjpindexphp
if you are interested in other research works
Thank you
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Itrsquos easy to calculate output motion of themanipulator
2 Kinematics of planar serial link mechanisms
Forward kinematics
+++++minusminus
=
)cos()(sin)sin()(sin
10101000
10101000
θθθθθθθθθθθθ
LLLL
YX
P
P
By differentiating the displacement the output velocity can be calculated as
Linear relationship between output velocity and joint velocity
It can be deformed as
++++minus+minusminus
=
1
0
10110100
10110100
)cos()cos(cos)sin()sin(sin
θθ
θθθθθθθθθθ
LLLLLL
YX
P
P
Jacobian matrix J(θ) Joint velocityvector θ
Output velocityvector r
θθJr )(=
θθJθθJ
)()(
)]sin()()cos()([
)sincos()]cos()()sin()([
)cossin(
102
1010101
020000
102
1010101
020000
+=
++minus+++
minus+++++minus
+minus
=
θθθθθθθθ
θθθθθθθθθθθθ
θθθθ
L
LL
L
YX
P
P
Output acceleration can also be calculated as
I t is easy to analyze output motion with knowledge at high school
For general planar serial nR manipulatorhellip
+
+=
sum sum
sum summinus
= =
minus
= =
0
1
0 0
0
1
0 0
sin
cos
YL
XL
YX
n
i
i
kki
n
i
i
kki
P
P
θ
θ
minus
=
sum sumsum
sum sumsumminus
= ==
minus
= ==1
0 00
1
0 00
cos
sinn
i
i
kk
i
kki
n
i
i
kk
i
kki
P
P
L
L
YX
θθ
θθ
minus
+minus=
sum sum sum sumsum
sum sum sum sumsumminus
= = = ==
minus
= = = ==1
0 0 0 0
2
0
1
0 0 0 0
2
0
]sin)(cos[
]cos)(sin[n
i
i
k
i
k
i
kkkk
i
kki
n
i
i
k
i
k
i
kkkk
i
kki
P
P
L
L
YX
θθθθ
θθθθ
Itrsquos also easy to analyze
Inverse kinematics
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
To calculate joint input motion forthe specified output motion
Can θ0θ1 be calculated as the function of XPYP
It will be a little bit complicated
By using the prementioned equation for forward kinematics
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Let draw an auxiliary line
By defining angles αβ
βαθ minus=0
where
0
01tanXXYY
P
P
minusminus
= minusα
How to derive β X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
α
β
Planar serial 2R manipulator
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
β
Let consider PJ0J1
L
20
20 )()( YYXXL PP minus+minus=
By using side lengths L0L1 anddistance
0
21
20
2
2cos
LLLLL minus+
=β
We can apply cosine theorem
0
21
20
21
2cos
LLLLL minus+
= minusβTherefore
Planar serial 2R manipulator
0
21
20
21
0
01
0
2costan
LLLLL
XXYY
P
P minus+minus
minusminus
=
minus=
minusminus
βαθ
θ1 can then be calculated
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1 L
Let consider PJ0J1 and an angle γ
γ
θ1=πminusγ
10
221
201
2cos
LLLLL minus+
= minusγ
From cosine theorem
10
221
201
1
2cos
LLLLL minus+
minus=
minus=
minusπ
γπθ
Joint input velocity and acceleration can be derived as
γθ
γθ
βαθ
βαθ
minus=
minus=
minus=
minus=
1
1
0
0
γγ
sin10LLLL
=
γγγγγ 2
10
2
sincossin)(
LLLLLLL
minus+
=
LYYXXL PPPP
+
=
2
22 )()(L
LYYXXLYYYXXXL PPPPPPPPPP
+minus+++=
They can also be derived as
+minusminus+minusminus
++=
++++minus+minusminus
=
=minus
minus
P
P
P
P
YX
LLLLLL
LL
YX
LLLLLL
)sin(sin)cos(cos)sin()cos(
sin1
)cos()cos(cos)sin()sin(sin
)(
1010010100
101101
110
1
10110100
10110100
1
θθθθθθθθθθ
θ
θθθθθθθθθθ
rθJθ
++minus++minusminus++minus++minusminus
minus
times
+minusminus+minusminus
++=
minus= minus
1
0
1010110101000
1010110101000
1010010100
101101
110
1
)sin()()sin()(sin)cos()()cos()(cos
)sin(sin)cos(cos)sin()cos(
sin1
])([)(
θθ
θθθθθθθθθθθθθθθθθθθθ
θθθθθθθθθθ
θ
LLLLLL
YX
LLLLLL
LL
P
P
θθJrθJθ
Anyway it is easy to analyze kinematics of serial mechanisms
3 Kinematics of planar closed-loop link mechanisms
The simplestclosed-loop mechanism
Planar 4-bar link mechanism
θ1L3L 2L
)( YX出力
Input link
X
Y φ
0L
Follower link
Coupler link
ξ
η
ξ
η
J0
J1
J2
J3
P
O
Crank-rocker motion
Planar 4-bar link mechanismX
Y
X
Y
X
Y
X
Y
X
Y
X
Y
X
Y
Planar 4-bar link mechanismX
Y
X
Y
)( YXOutput coupler point
X
Y
O
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Itrsquos easy to calculate output motion of themanipulator
2 Kinematics of planar serial link mechanisms
Forward kinematics
+++++minusminus
=
)cos()(sin)sin()(sin
10101000
10101000
θθθθθθθθθθθθ
LLLL
YX
P
P
By differentiating the displacement the output velocity can be calculated as
Linear relationship between output velocity and joint velocity
It can be deformed as
++++minus+minusminus
=
1
0
10110100
10110100
)cos()cos(cos)sin()sin(sin
θθ
θθθθθθθθθθ
LLLLLL
YX
P
P
Jacobian matrix J(θ) Joint velocityvector θ
Output velocityvector r
θθJr )(=
θθJθθJ
)()(
)]sin()()cos()([
)sincos()]cos()()sin()([
)cossin(
102
1010101
020000
102
1010101
020000
+=
++minus+++
minus+++++minus
+minus
=
θθθθθθθθ
θθθθθθθθθθθθ
θθθθ
L
LL
L
YX
P
P
Output acceleration can also be calculated as
I t is easy to analyze output motion with knowledge at high school
For general planar serial nR manipulatorhellip
+
+=
sum sum
sum summinus
= =
minus
= =
0
1
0 0
0
1
0 0
sin
cos
YL
XL
YX
n
i
i
kki
n
i
i
kki
P
P
θ
θ
minus
=
sum sumsum
sum sumsumminus
= ==
minus
= ==1
0 00
1
0 00
cos
sinn
i
i
kk
i
kki
n
i
i
kk
i
kki
P
P
L
L
YX
θθ
θθ
minus
+minus=
sum sum sum sumsum
sum sum sum sumsumminus
= = = ==
minus
= = = ==1
0 0 0 0
2
0
1
0 0 0 0
2
0
]sin)(cos[
]cos)(sin[n
i
i
k
i
k
i
kkkk
i
kki
n
i
i
k
i
k
i
kkkk
i
kki
P
P
L
L
YX
θθθθ
θθθθ
Itrsquos also easy to analyze
Inverse kinematics
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
To calculate joint input motion forthe specified output motion
Can θ0θ1 be calculated as the function of XPYP
It will be a little bit complicated
By using the prementioned equation for forward kinematics
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Let draw an auxiliary line
By defining angles αβ
βαθ minus=0
where
0
01tanXXYY
P
P
minusminus
= minusα
How to derive β X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
α
β
Planar serial 2R manipulator
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
β
Let consider PJ0J1
L
20
20 )()( YYXXL PP minus+minus=
By using side lengths L0L1 anddistance
0
21
20
2
2cos
LLLLL minus+
=β
We can apply cosine theorem
0
21
20
21
2cos
LLLLL minus+
= minusβTherefore
Planar serial 2R manipulator
0
21
20
21
0
01
0
2costan
LLLLL
XXYY
P
P minus+minus
minusminus
=
minus=
minusminus
βαθ
θ1 can then be calculated
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1 L
Let consider PJ0J1 and an angle γ
γ
θ1=πminusγ
10
221
201
2cos
LLLLL minus+
= minusγ
From cosine theorem
10
221
201
1
2cos
LLLLL minus+
minus=
minus=
minusπ
γπθ
Joint input velocity and acceleration can be derived as
γθ
γθ
βαθ
βαθ
minus=
minus=
minus=
minus=
1
1
0
0
γγ
sin10LLLL
=
γγγγγ 2
10
2
sincossin)(
LLLLLLL
minus+
=
LYYXXL PPPP
+
=
2
22 )()(L
LYYXXLYYYXXXL PPPPPPPPPP
+minus+++=
They can also be derived as
+minusminus+minusminus
++=
++++minus+minusminus
=
=minus
minus
P
P
P
P
YX
LLLLLL
LL
YX
LLLLLL
)sin(sin)cos(cos)sin()cos(
sin1
)cos()cos(cos)sin()sin(sin
)(
1010010100
101101
110
1
10110100
10110100
1
θθθθθθθθθθ
θ
θθθθθθθθθθ
rθJθ
++minus++minusminus++minus++minusminus
minus
times
+minusminus+minusminus
++=
minus= minus
1
0
1010110101000
1010110101000
1010010100
101101
110
1
)sin()()sin()(sin)cos()()cos()(cos
)sin(sin)cos(cos)sin()cos(
sin1
])([)(
θθ
θθθθθθθθθθθθθθθθθθθθ
θθθθθθθθθθ
θ
LLLLLL
YX
LLLLLL
LL
P
P
θθJrθJθ
Anyway it is easy to analyze kinematics of serial mechanisms
3 Kinematics of planar closed-loop link mechanisms
The simplestclosed-loop mechanism
Planar 4-bar link mechanism
θ1L3L 2L
)( YX出力
Input link
X
Y φ
0L
Follower link
Coupler link
ξ
η
ξ
η
J0
J1
J2
J3
P
O
Crank-rocker motion
Planar 4-bar link mechanismX
Y
X
Y
X
Y
X
Y
X
Y
X
Y
X
Y
Planar 4-bar link mechanismX
Y
X
Y
)( YXOutput coupler point
X
Y
O
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
+++++minusminus
=
)cos()(sin)sin()(sin
10101000
10101000
θθθθθθθθθθθθ
LLLL
YX
P
P
By differentiating the displacement the output velocity can be calculated as
Linear relationship between output velocity and joint velocity
It can be deformed as
++++minus+minusminus
=
1
0
10110100
10110100
)cos()cos(cos)sin()sin(sin
θθ
θθθθθθθθθθ
LLLLLL
YX
P
P
Jacobian matrix J(θ) Joint velocityvector θ
Output velocityvector r
θθJr )(=
θθJθθJ
)()(
)]sin()()cos()([
)sincos()]cos()()sin()([
)cossin(
102
1010101
020000
102
1010101
020000
+=
++minus+++
minus+++++minus
+minus
=
θθθθθθθθ
θθθθθθθθθθθθ
θθθθ
L
LL
L
YX
P
P
Output acceleration can also be calculated as
I t is easy to analyze output motion with knowledge at high school
For general planar serial nR manipulatorhellip
+
+=
sum sum
sum summinus
= =
minus
= =
0
1
0 0
0
1
0 0
sin
cos
YL
XL
YX
n
i
i
kki
n
i
i
kki
P
P
θ
θ
minus
=
sum sumsum
sum sumsumminus
= ==
minus
= ==1
0 00
1
0 00
cos
sinn
i
i
kk
i
kki
n
i
i
kk
i
kki
P
P
L
L
YX
θθ
θθ
minus
+minus=
sum sum sum sumsum
sum sum sum sumsumminus
= = = ==
minus
= = = ==1
0 0 0 0
2
0
1
0 0 0 0
2
0
]sin)(cos[
]cos)(sin[n
i
i
k
i
k
i
kkkk
i
kki
n
i
i
k
i
k
i
kkkk
i
kki
P
P
L
L
YX
θθθθ
θθθθ
Itrsquos also easy to analyze
Inverse kinematics
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
To calculate joint input motion forthe specified output motion
Can θ0θ1 be calculated as the function of XPYP
It will be a little bit complicated
By using the prementioned equation for forward kinematics
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Let draw an auxiliary line
By defining angles αβ
βαθ minus=0
where
0
01tanXXYY
P
P
minusminus
= minusα
How to derive β X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
α
β
Planar serial 2R manipulator
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
β
Let consider PJ0J1
L
20
20 )()( YYXXL PP minus+minus=
By using side lengths L0L1 anddistance
0
21
20
2
2cos
LLLLL minus+
=β
We can apply cosine theorem
0
21
20
21
2cos
LLLLL minus+
= minusβTherefore
Planar serial 2R manipulator
0
21
20
21
0
01
0
2costan
LLLLL
XXYY
P
P minus+minus
minusminus
=
minus=
minusminus
βαθ
θ1 can then be calculated
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1 L
Let consider PJ0J1 and an angle γ
γ
θ1=πminusγ
10
221
201
2cos
LLLLL minus+
= minusγ
From cosine theorem
10
221
201
1
2cos
LLLLL minus+
minus=
minus=
minusπ
γπθ
Joint input velocity and acceleration can be derived as
γθ
γθ
βαθ
βαθ
minus=
minus=
minus=
minus=
1
1
0
0
γγ
sin10LLLL
=
γγγγγ 2
10
2
sincossin)(
LLLLLLL
minus+
=
LYYXXL PPPP
+
=
2
22 )()(L
LYYXXLYYYXXXL PPPPPPPPPP
+minus+++=
They can also be derived as
+minusminus+minusminus
++=
++++minus+minusminus
=
=minus
minus
P
P
P
P
YX
LLLLLL
LL
YX
LLLLLL
)sin(sin)cos(cos)sin()cos(
sin1
)cos()cos(cos)sin()sin(sin
)(
1010010100
101101
110
1
10110100
10110100
1
θθθθθθθθθθ
θ
θθθθθθθθθθ
rθJθ
++minus++minusminus++minus++minusminus
minus
times
+minusminus+minusminus
++=
minus= minus
1
0
1010110101000
1010110101000
1010010100
101101
110
1
)sin()()sin()(sin)cos()()cos()(cos
)sin(sin)cos(cos)sin()cos(
sin1
])([)(
θθ
θθθθθθθθθθθθθθθθθθθθ
θθθθθθθθθθ
θ
LLLLLL
YX
LLLLLL
LL
P
P
θθJrθJθ
Anyway it is easy to analyze kinematics of serial mechanisms
3 Kinematics of planar closed-loop link mechanisms
The simplestclosed-loop mechanism
Planar 4-bar link mechanism
θ1L3L 2L
)( YX出力
Input link
X
Y φ
0L
Follower link
Coupler link
ξ
η
ξ
η
J0
J1
J2
J3
P
O
Crank-rocker motion
Planar 4-bar link mechanismX
Y
X
Y
X
Y
X
Y
X
Y
X
Y
X
Y
Planar 4-bar link mechanismX
Y
X
Y
)( YXOutput coupler point
X
Y
O
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
θθJθθJ
)()(
)]sin()()cos()([
)sincos()]cos()()sin()([
)cossin(
102
1010101
020000
102
1010101
020000
+=
++minus+++
minus+++++minus
+minus
=
θθθθθθθθ
θθθθθθθθθθθθ
θθθθ
L
LL
L
YX
P
P
Output acceleration can also be calculated as
I t is easy to analyze output motion with knowledge at high school
For general planar serial nR manipulatorhellip
+
+=
sum sum
sum summinus
= =
minus
= =
0
1
0 0
0
1
0 0
sin
cos
YL
XL
YX
n
i
i
kki
n
i
i
kki
P
P
θ
θ
minus
=
sum sumsum
sum sumsumminus
= ==
minus
= ==1
0 00
1
0 00
cos
sinn
i
i
kk
i
kki
n
i
i
kk
i
kki
P
P
L
L
YX
θθ
θθ
minus
+minus=
sum sum sum sumsum
sum sum sum sumsumminus
= = = ==
minus
= = = ==1
0 0 0 0
2
0
1
0 0 0 0
2
0
]sin)(cos[
]cos)(sin[n
i
i
k
i
k
i
kkkk
i
kki
n
i
i
k
i
k
i
kkkk
i
kki
P
P
L
L
YX
θθθθ
θθθθ
Itrsquos also easy to analyze
Inverse kinematics
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
To calculate joint input motion forthe specified output motion
Can θ0θ1 be calculated as the function of XPYP
It will be a little bit complicated
By using the prementioned equation for forward kinematics
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Let draw an auxiliary line
By defining angles αβ
βαθ minus=0
where
0
01tanXXYY
P
P
minusminus
= minusα
How to derive β X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
α
β
Planar serial 2R manipulator
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
β
Let consider PJ0J1
L
20
20 )()( YYXXL PP minus+minus=
By using side lengths L0L1 anddistance
0
21
20
2
2cos
LLLLL minus+
=β
We can apply cosine theorem
0
21
20
21
2cos
LLLLL minus+
= minusβTherefore
Planar serial 2R manipulator
0
21
20
21
0
01
0
2costan
LLLLL
XXYY
P
P minus+minus
minusminus
=
minus=
minusminus
βαθ
θ1 can then be calculated
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1 L
Let consider PJ0J1 and an angle γ
γ
θ1=πminusγ
10
221
201
2cos
LLLLL minus+
= minusγ
From cosine theorem
10
221
201
1
2cos
LLLLL minus+
minus=
minus=
minusπ
γπθ
Joint input velocity and acceleration can be derived as
γθ
γθ
βαθ
βαθ
minus=
minus=
minus=
minus=
1
1
0
0
γγ
sin10LLLL
=
γγγγγ 2
10
2
sincossin)(
LLLLLLL
minus+
=
LYYXXL PPPP
+
=
2
22 )()(L
LYYXXLYYYXXXL PPPPPPPPPP
+minus+++=
They can also be derived as
+minusminus+minusminus
++=
++++minus+minusminus
=
=minus
minus
P
P
P
P
YX
LLLLLL
LL
YX
LLLLLL
)sin(sin)cos(cos)sin()cos(
sin1
)cos()cos(cos)sin()sin(sin
)(
1010010100
101101
110
1
10110100
10110100
1
θθθθθθθθθθ
θ
θθθθθθθθθθ
rθJθ
++minus++minusminus++minus++minusminus
minus
times
+minusminus+minusminus
++=
minus= minus
1
0
1010110101000
1010110101000
1010010100
101101
110
1
)sin()()sin()(sin)cos()()cos()(cos
)sin(sin)cos(cos)sin()cos(
sin1
])([)(
θθ
θθθθθθθθθθθθθθθθθθθθ
θθθθθθθθθθ
θ
LLLLLL
YX
LLLLLL
LL
P
P
θθJrθJθ
Anyway it is easy to analyze kinematics of serial mechanisms
3 Kinematics of planar closed-loop link mechanisms
The simplestclosed-loop mechanism
Planar 4-bar link mechanism
θ1L3L 2L
)( YX出力
Input link
X
Y φ
0L
Follower link
Coupler link
ξ
η
ξ
η
J0
J1
J2
J3
P
O
Crank-rocker motion
Planar 4-bar link mechanismX
Y
X
Y
X
Y
X
Y
X
Y
X
Y
X
Y
Planar 4-bar link mechanismX
Y
X
Y
)( YXOutput coupler point
X
Y
O
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
For general planar serial nR manipulatorhellip
+
+=
sum sum
sum summinus
= =
minus
= =
0
1
0 0
0
1
0 0
sin
cos
YL
XL
YX
n
i
i
kki
n
i
i
kki
P
P
θ
θ
minus
=
sum sumsum
sum sumsumminus
= ==
minus
= ==1
0 00
1
0 00
cos
sinn
i
i
kk
i
kki
n
i
i
kk
i
kki
P
P
L
L
YX
θθ
θθ
minus
+minus=
sum sum sum sumsum
sum sum sum sumsumminus
= = = ==
minus
= = = ==1
0 0 0 0
2
0
1
0 0 0 0
2
0
]sin)(cos[
]cos)(sin[n
i
i
k
i
k
i
kkkk
i
kki
n
i
i
k
i
k
i
kkkk
i
kki
P
P
L
L
YX
θθθθ
θθθθ
Itrsquos also easy to analyze
Inverse kinematics
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
To calculate joint input motion forthe specified output motion
Can θ0θ1 be calculated as the function of XPYP
It will be a little bit complicated
By using the prementioned equation for forward kinematics
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Let draw an auxiliary line
By defining angles αβ
βαθ minus=0
where
0
01tanXXYY
P
P
minusminus
= minusα
How to derive β X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
α
β
Planar serial 2R manipulator
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
β
Let consider PJ0J1
L
20
20 )()( YYXXL PP minus+minus=
By using side lengths L0L1 anddistance
0
21
20
2
2cos
LLLLL minus+
=β
We can apply cosine theorem
0
21
20
21
2cos
LLLLL minus+
= minusβTherefore
Planar serial 2R manipulator
0
21
20
21
0
01
0
2costan
LLLLL
XXYY
P
P minus+minus
minusminus
=
minus=
minusminus
βαθ
θ1 can then be calculated
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1 L
Let consider PJ0J1 and an angle γ
γ
θ1=πminusγ
10
221
201
2cos
LLLLL minus+
= minusγ
From cosine theorem
10
221
201
1
2cos
LLLLL minus+
minus=
minus=
minusπ
γπθ
Joint input velocity and acceleration can be derived as
γθ
γθ
βαθ
βαθ
minus=
minus=
minus=
minus=
1
1
0
0
γγ
sin10LLLL
=
γγγγγ 2
10
2
sincossin)(
LLLLLLL
minus+
=
LYYXXL PPPP
+
=
2
22 )()(L
LYYXXLYYYXXXL PPPPPPPPPP
+minus+++=
They can also be derived as
+minusminus+minusminus
++=
++++minus+minusminus
=
=minus
minus
P
P
P
P
YX
LLLLLL
LL
YX
LLLLLL
)sin(sin)cos(cos)sin()cos(
sin1
)cos()cos(cos)sin()sin(sin
)(
1010010100
101101
110
1
10110100
10110100
1
θθθθθθθθθθ
θ
θθθθθθθθθθ
rθJθ
++minus++minusminus++minus++minusminus
minus
times
+minusminus+minusminus
++=
minus= minus
1
0
1010110101000
1010110101000
1010010100
101101
110
1
)sin()()sin()(sin)cos()()cos()(cos
)sin(sin)cos(cos)sin()cos(
sin1
])([)(
θθ
θθθθθθθθθθθθθθθθθθθθ
θθθθθθθθθθ
θ
LLLLLL
YX
LLLLLL
LL
P
P
θθJrθJθ
Anyway it is easy to analyze kinematics of serial mechanisms
3 Kinematics of planar closed-loop link mechanisms
The simplestclosed-loop mechanism
Planar 4-bar link mechanism
θ1L3L 2L
)( YX出力
Input link
X
Y φ
0L
Follower link
Coupler link
ξ
η
ξ
η
J0
J1
J2
J3
P
O
Crank-rocker motion
Planar 4-bar link mechanismX
Y
X
Y
X
Y
X
Y
X
Y
X
Y
X
Y
Planar 4-bar link mechanismX
Y
X
Y
)( YXOutput coupler point
X
Y
O
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
Inverse kinematics
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
To calculate joint input motion forthe specified output motion
Can θ0θ1 be calculated as the function of XPYP
It will be a little bit complicated
By using the prementioned equation for forward kinematics
++++++
=
010100
010100
)sin(sin)cos(cos
YLLXLL
YX
P
P
θθθθθθ
Planar serial 2R manipulator
Let draw an auxiliary line
By defining angles αβ
βαθ minus=0
where
0
01tanXXYY
P
P
minusminus
= minusα
How to derive β X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
α
β
Planar serial 2R manipulator
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
β
Let consider PJ0J1
L
20
20 )()( YYXXL PP minus+minus=
By using side lengths L0L1 anddistance
0
21
20
2
2cos
LLLLL minus+
=β
We can apply cosine theorem
0
21
20
21
2cos
LLLLL minus+
= minusβTherefore
Planar serial 2R manipulator
0
21
20
21
0
01
0
2costan
LLLLL
XXYY
P
P minus+minus
minusminus
=
minus=
minusminus
βαθ
θ1 can then be calculated
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1 L
Let consider PJ0J1 and an angle γ
γ
θ1=πminusγ
10
221
201
2cos
LLLLL minus+
= minusγ
From cosine theorem
10
221
201
1
2cos
LLLLL minus+
minus=
minus=
minusπ
γπθ
Joint input velocity and acceleration can be derived as
γθ
γθ
βαθ
βαθ
minus=
minus=
minus=
minus=
1
1
0
0
γγ
sin10LLLL
=
γγγγγ 2
10
2
sincossin)(
LLLLLLL
minus+
=
LYYXXL PPPP
+
=
2
22 )()(L
LYYXXLYYYXXXL PPPPPPPPPP
+minus+++=
They can also be derived as
+minusminus+minusminus
++=
++++minus+minusminus
=
=minus
minus
P
P
P
P
YX
LLLLLL
LL
YX
LLLLLL
)sin(sin)cos(cos)sin()cos(
sin1
)cos()cos(cos)sin()sin(sin
)(
1010010100
101101
110
1
10110100
10110100
1
θθθθθθθθθθ
θ
θθθθθθθθθθ
rθJθ
++minus++minusminus++minus++minusminus
minus
times
+minusminus+minusminus
++=
minus= minus
1
0
1010110101000
1010110101000
1010010100
101101
110
1
)sin()()sin()(sin)cos()()cos()(cos
)sin(sin)cos(cos)sin()cos(
sin1
])([)(
θθ
θθθθθθθθθθθθθθθθθθθθ
θθθθθθθθθθ
θ
LLLLLL
YX
LLLLLL
LL
P
P
θθJrθJθ
Anyway it is easy to analyze kinematics of serial mechanisms
3 Kinematics of planar closed-loop link mechanisms
The simplestclosed-loop mechanism
Planar 4-bar link mechanism
θ1L3L 2L
)( YX出力
Input link
X
Y φ
0L
Follower link
Coupler link
ξ
η
ξ
η
J0
J1
J2
J3
P
O
Crank-rocker motion
Planar 4-bar link mechanismX
Y
X
Y
X
Y
X
Y
X
Y
X
Y
X
Y
Planar 4-bar link mechanismX
Y
X
Y
)( YXOutput coupler point
X
Y
O
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
Let draw an auxiliary line
By defining angles αβ
βαθ minus=0
where
0
01tanXXYY
P
P
minusminus
= minusα
How to derive β X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
α
β
Planar serial 2R manipulator
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
β
Let consider PJ0J1
L
20
20 )()( YYXXL PP minus+minus=
By using side lengths L0L1 anddistance
0
21
20
2
2cos
LLLLL minus+
=β
We can apply cosine theorem
0
21
20
21
2cos
LLLLL minus+
= minusβTherefore
Planar serial 2R manipulator
0
21
20
21
0
01
0
2costan
LLLLL
XXYY
P
P minus+minus
minusminus
=
minus=
minusminus
βαθ
θ1 can then be calculated
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1 L
Let consider PJ0J1 and an angle γ
γ
θ1=πminusγ
10
221
201
2cos
LLLLL minus+
= minusγ
From cosine theorem
10
221
201
1
2cos
LLLLL minus+
minus=
minus=
minusπ
γπθ
Joint input velocity and acceleration can be derived as
γθ
γθ
βαθ
βαθ
minus=
minus=
minus=
minus=
1
1
0
0
γγ
sin10LLLL
=
γγγγγ 2
10
2
sincossin)(
LLLLLLL
minus+
=
LYYXXL PPPP
+
=
2
22 )()(L
LYYXXLYYYXXXL PPPPPPPPPP
+minus+++=
They can also be derived as
+minusminus+minusminus
++=
++++minus+minusminus
=
=minus
minus
P
P
P
P
YX
LLLLLL
LL
YX
LLLLLL
)sin(sin)cos(cos)sin()cos(
sin1
)cos()cos(cos)sin()sin(sin
)(
1010010100
101101
110
1
10110100
10110100
1
θθθθθθθθθθ
θ
θθθθθθθθθθ
rθJθ
++minus++minusminus++minus++minusminus
minus
times
+minusminus+minusminus
++=
minus= minus
1
0
1010110101000
1010110101000
1010010100
101101
110
1
)sin()()sin()(sin)cos()()cos()(cos
)sin(sin)cos(cos)sin()cos(
sin1
])([)(
θθ
θθθθθθθθθθθθθθθθθθθθ
θθθθθθθθθθ
θ
LLLLLL
YX
LLLLLL
LL
P
P
θθJrθJθ
Anyway it is easy to analyze kinematics of serial mechanisms
3 Kinematics of planar closed-loop link mechanisms
The simplestclosed-loop mechanism
Planar 4-bar link mechanism
θ1L3L 2L
)( YX出力
Input link
X
Y φ
0L
Follower link
Coupler link
ξ
η
ξ
η
J0
J1
J2
J3
P
O
Crank-rocker motion
Planar 4-bar link mechanismX
Y
X
Y
X
Y
X
Y
X
Y
X
Y
X
Y
Planar 4-bar link mechanismX
Y
X
Y
)( YXOutput coupler point
X
Y
O
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1
β
Let consider PJ0J1
L
20
20 )()( YYXXL PP minus+minus=
By using side lengths L0L1 anddistance
0
21
20
2
2cos
LLLLL minus+
=β
We can apply cosine theorem
0
21
20
21
2cos
LLLLL minus+
= minusβTherefore
Planar serial 2R manipulator
0
21
20
21
0
01
0
2costan
LLLLL
XXYY
P
P minus+minus
minusminus
=
minus=
minusminus
βαθ
θ1 can then be calculated
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1 L
Let consider PJ0J1 and an angle γ
γ
θ1=πminusγ
10
221
201
2cos
LLLLL minus+
= minusγ
From cosine theorem
10
221
201
1
2cos
LLLLL minus+
minus=
minus=
minusπ
γπθ
Joint input velocity and acceleration can be derived as
γθ
γθ
βαθ
βαθ
minus=
minus=
minus=
minus=
1
1
0
0
γγ
sin10LLLL
=
γγγγγ 2
10
2
sincossin)(
LLLLLLL
minus+
=
LYYXXL PPPP
+
=
2
22 )()(L
LYYXXLYYYXXXL PPPPPPPPPP
+minus+++=
They can also be derived as
+minusminus+minusminus
++=
++++minus+minusminus
=
=minus
minus
P
P
P
P
YX
LLLLLL
LL
YX
LLLLLL
)sin(sin)cos(cos)sin()cos(
sin1
)cos()cos(cos)sin()sin(sin
)(
1010010100
101101
110
1
10110100
10110100
1
θθθθθθθθθθ
θ
θθθθθθθθθθ
rθJθ
++minus++minusminus++minus++minusminus
minus
times
+minusminus+minusminus
++=
minus= minus
1
0
1010110101000
1010110101000
1010010100
101101
110
1
)sin()()sin()(sin)cos()()cos()(cos
)sin(sin)cos(cos)sin()cos(
sin1
])([)(
θθ
θθθθθθθθθθθθθθθθθθθθ
θθθθθθθθθθ
θ
LLLLLL
YX
LLLLLL
LL
P
P
θθJrθJθ
Anyway it is easy to analyze kinematics of serial mechanisms
3 Kinematics of planar closed-loop link mechanisms
The simplestclosed-loop mechanism
Planar 4-bar link mechanism
θ1L3L 2L
)( YX出力
Input link
X
Y φ
0L
Follower link
Coupler link
ξ
η
ξ
η
J0
J1
J2
J3
P
O
Crank-rocker motion
Planar 4-bar link mechanismX
Y
X
Y
X
Y
X
Y
X
Y
X
Y
X
Y
Planar 4-bar link mechanismX
Y
X
Y
)( YXOutput coupler point
X
Y
O
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
0
21
20
21
0
01
0
2costan
LLLLL
XXYY
P
P minus+minus
minusminus
=
minus=
minusminus
βαθ
θ1 can then be calculated
X
J0 ( X0 Y0 )
θ 0
X
Y
O
J1 ( X1 Y1)
P ( XP YP )
θ 1
L0
L1 L
Let consider PJ0J1 and an angle γ
γ
θ1=πminusγ
10
221
201
2cos
LLLLL minus+
= minusγ
From cosine theorem
10
221
201
1
2cos
LLLLL minus+
minus=
minus=
minusπ
γπθ
Joint input velocity and acceleration can be derived as
γθ
γθ
βαθ
βαθ
minus=
minus=
minus=
minus=
1
1
0
0
γγ
sin10LLLL
=
γγγγγ 2
10
2
sincossin)(
LLLLLLL
minus+
=
LYYXXL PPPP
+
=
2
22 )()(L
LYYXXLYYYXXXL PPPPPPPPPP
+minus+++=
They can also be derived as
+minusminus+minusminus
++=
++++minus+minusminus
=
=minus
minus
P
P
P
P
YX
LLLLLL
LL
YX
LLLLLL
)sin(sin)cos(cos)sin()cos(
sin1
)cos()cos(cos)sin()sin(sin
)(
1010010100
101101
110
1
10110100
10110100
1
θθθθθθθθθθ
θ
θθθθθθθθθθ
rθJθ
++minus++minusminus++minus++minusminus
minus
times
+minusminus+minusminus
++=
minus= minus
1
0
1010110101000
1010110101000
1010010100
101101
110
1
)sin()()sin()(sin)cos()()cos()(cos
)sin(sin)cos(cos)sin()cos(
sin1
])([)(
θθ
θθθθθθθθθθθθθθθθθθθθ
θθθθθθθθθθ
θ
LLLLLL
YX
LLLLLL
LL
P
P
θθJrθJθ
Anyway it is easy to analyze kinematics of serial mechanisms
3 Kinematics of planar closed-loop link mechanisms
The simplestclosed-loop mechanism
Planar 4-bar link mechanism
θ1L3L 2L
)( YX出力
Input link
X
Y φ
0L
Follower link
Coupler link
ξ
η
ξ
η
J0
J1
J2
J3
P
O
Crank-rocker motion
Planar 4-bar link mechanismX
Y
X
Y
X
Y
X
Y
X
Y
X
Y
X
Y
Planar 4-bar link mechanismX
Y
X
Y
)( YXOutput coupler point
X
Y
O
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
10
221
201
1
2cos
LLLLL minus+
minus=
minus=
minusπ
γπθ
Joint input velocity and acceleration can be derived as
γθ
γθ
βαθ
βαθ
minus=
minus=
minus=
minus=
1
1
0
0
γγ
sin10LLLL
=
γγγγγ 2
10
2
sincossin)(
LLLLLLL
minus+
=
LYYXXL PPPP
+
=
2
22 )()(L
LYYXXLYYYXXXL PPPPPPPPPP
+minus+++=
They can also be derived as
+minusminus+minusminus
++=
++++minus+minusminus
=
=minus
minus
P
P
P
P
YX
LLLLLL
LL
YX
LLLLLL
)sin(sin)cos(cos)sin()cos(
sin1
)cos()cos(cos)sin()sin(sin
)(
1010010100
101101
110
1
10110100
10110100
1
θθθθθθθθθθ
θ
θθθθθθθθθθ
rθJθ
++minus++minusminus++minus++minusminus
minus
times
+minusminus+minusminus
++=
minus= minus
1
0
1010110101000
1010110101000
1010010100
101101
110
1
)sin()()sin()(sin)cos()()cos()(cos
)sin(sin)cos(cos)sin()cos(
sin1
])([)(
θθ
θθθθθθθθθθθθθθθθθθθθ
θθθθθθθθθθ
θ
LLLLLL
YX
LLLLLL
LL
P
P
θθJrθJθ
Anyway it is easy to analyze kinematics of serial mechanisms
3 Kinematics of planar closed-loop link mechanisms
The simplestclosed-loop mechanism
Planar 4-bar link mechanism
θ1L3L 2L
)( YX出力
Input link
X
Y φ
0L
Follower link
Coupler link
ξ
η
ξ
η
J0
J1
J2
J3
P
O
Crank-rocker motion
Planar 4-bar link mechanismX
Y
X
Y
X
Y
X
Y
X
Y
X
Y
X
Y
Planar 4-bar link mechanismX
Y
X
Y
)( YXOutput coupler point
X
Y
O
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
γγ
sin10LLLL
=
γγγγγ 2
10
2
sincossin)(
LLLLLLL
minus+
=
LYYXXL PPPP
+
=
2
22 )()(L
LYYXXLYYYXXXL PPPPPPPPPP
+minus+++=
They can also be derived as
+minusminus+minusminus
++=
++++minus+minusminus
=
=minus
minus
P
P
P
P
YX
LLLLLL
LL
YX
LLLLLL
)sin(sin)cos(cos)sin()cos(
sin1
)cos()cos(cos)sin()sin(sin
)(
1010010100
101101
110
1
10110100
10110100
1
θθθθθθθθθθ
θ
θθθθθθθθθθ
rθJθ
++minus++minusminus++minus++minusminus
minus
times
+minusminus+minusminus
++=
minus= minus
1
0
1010110101000
1010110101000
1010010100
101101
110
1
)sin()()sin()(sin)cos()()cos()(cos
)sin(sin)cos(cos)sin()cos(
sin1
])([)(
θθ
θθθθθθθθθθθθθθθθθθθθ
θθθθθθθθθθ
θ
LLLLLL
YX
LLLLLL
LL
P
P
θθJrθJθ
Anyway it is easy to analyze kinematics of serial mechanisms
3 Kinematics of planar closed-loop link mechanisms
The simplestclosed-loop mechanism
Planar 4-bar link mechanism
θ1L3L 2L
)( YX出力
Input link
X
Y φ
0L
Follower link
Coupler link
ξ
η
ξ
η
J0
J1
J2
J3
P
O
Crank-rocker motion
Planar 4-bar link mechanismX
Y
X
Y
X
Y
X
Y
X
Y
X
Y
X
Y
Planar 4-bar link mechanismX
Y
X
Y
)( YXOutput coupler point
X
Y
O
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
They can also be derived as
+minusminus+minusminus
++=
++++minus+minusminus
=
=minus
minus
P
P
P
P
YX
LLLLLL
LL
YX
LLLLLL
)sin(sin)cos(cos)sin()cos(
sin1
)cos()cos(cos)sin()sin(sin
)(
1010010100
101101
110
1
10110100
10110100
1
θθθθθθθθθθ
θ
θθθθθθθθθθ
rθJθ
++minus++minusminus++minus++minusminus
minus
times
+minusminus+minusminus
++=
minus= minus
1
0
1010110101000
1010110101000
1010010100
101101
110
1
)sin()()sin()(sin)cos()()cos()(cos
)sin(sin)cos(cos)sin()cos(
sin1
])([)(
θθ
θθθθθθθθθθθθθθθθθθθθ
θθθθθθθθθθ
θ
LLLLLL
YX
LLLLLL
LL
P
P
θθJrθJθ
Anyway it is easy to analyze kinematics of serial mechanisms
3 Kinematics of planar closed-loop link mechanisms
The simplestclosed-loop mechanism
Planar 4-bar link mechanism
θ1L3L 2L
)( YX出力
Input link
X
Y φ
0L
Follower link
Coupler link
ξ
η
ξ
η
J0
J1
J2
J3
P
O
Crank-rocker motion
Planar 4-bar link mechanismX
Y
X
Y
X
Y
X
Y
X
Y
X
Y
X
Y
Planar 4-bar link mechanismX
Y
X
Y
)( YXOutput coupler point
X
Y
O
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
3 Kinematics of planar closed-loop link mechanisms
The simplestclosed-loop mechanism
Planar 4-bar link mechanism
θ1L3L 2L
)( YX出力
Input link
X
Y φ
0L
Follower link
Coupler link
ξ
η
ξ
η
J0
J1
J2
J3
P
O
Crank-rocker motion
Planar 4-bar link mechanismX
Y
X
Y
X
Y
X
Y
X
Y
X
Y
X
Y
Planar 4-bar link mechanismX
Y
X
Y
)( YXOutput coupler point
X
Y
O
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
Crank-rocker motion
Planar 4-bar link mechanismX
Y
X
Y
X
Y
X
Y
X
Y
X
Y
X
Y
Planar 4-bar link mechanismX
Y
X
Y
)( YXOutput coupler point
X
Y
O
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
Planar 4-bar link mechanism
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O
Let analyze output angular motion
)( 00 YX)( 03 YX
By eliminating θ1θ2 φ=f(θ) may be derived
Complicated
πφθθθθ
θθθ
+=+++=
+++=
2
2312100
2312103
sinsinsincoscoscos
LLLYYLLLXX
(1)From closed-loop equation
θ1
θ2
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
Planar 4-bar link mechanism
Let analyze output angular motion
θ1L3L 2L
)( YX
X
Yφ
0Lξ
η
ξ
η
J0
J1
J2
J3
P
O)( 00 YX)( 03 YX
By deforming equation with t=tan φ 2 and solving the equation to obtain t then φ=f(θ) may be derived
Complicated
(2)From length of coupler link
+=+=
++
=
=minus+minus
332
332
01
01
1
1
22
221
221
sincos
sincos
)()(
YLYXLX
YLXL
YX
LYYXX
φφ
θθ
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
θ1L
3L 2L
X
Y
φ
0L J0
J1
J2
J3
)( 11 YX
)( 22 YX
α
β
βαφ +=Let derive α and β respectively
(3)Let draw an auxiliary line
)( 00 YX)( 03 YXPlanar 4-bar link mechanism
Input link
Follower link Coupler link
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YX
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
θ1L
J1
J1
J3
)( 11 YX
α
θθ 221
2301 sin)cos( LXXLL +minus+=
3L2L
J1
J2
J3
)( 11 YX
)( 22 YX
β
301
11
cossintan
XXLL
minus+= minus
θθα
Easy
=βLet consider J3J1J2
Cosine theorem
3
22
23
21
2cos
LLLLL minus+
= minusβ
)( 00 YX)( 03 YX
)( 03 YXθ
θ
θθφ
cos)(2)(2cos)(2)(cos
cossintan
3012
30212
30123
22
230
211
301
11
XXLXXLLXXLLLXXL
XXLL
minus+minus+
minus++minusminus++
minus+=
minus
minus
Resultantly
φ can be represented as function of θ
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
Displacement of coupler point
3L
)( YXCoupler point
X
Y φ
Follwerlink
Coupler linkξ
η
ξ
η
J1
J2
J3
ψ
++
=
01
01
1
1
sincos
YLXL
YX
θθ
++
=
03
33
2
2
)(sin)(cos
YLXL
YX
θφθφAngle ψ
3031
311
21
211
)(coscos)(sinsintan
tan
XXLLLL
XXYY
minus+minusminus
=
minusminus
=
minus
minus
θφθθφθ
ψ
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
P
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
+++minus
=
+
minus=
2
2
2
2
cossinsincos
cossinsincos
)()(
YX
YX
YX
ψηψξψηψξ
ηξ
ψψψψ
θθ
)( YXCoupler point
X
YCoupler link
ξ
η
ξ
η
J1
J2ψ
Coordinate transformation
PResultantlyDisplacement of coupler point can be derived
HoweverhellipPeculiar to a mechanismIt will be complicated to analyze velocity and
acceleration
ldquoGeneral and systematic method is strongly requiredrdquo
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
4 Systematic kinematics analysis method
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
sCrank
Two adjacent linkswith revolute pairs
Two adjacent links withprismatic and revolute pairs
Coupler point
Crank input-Slider output planar 6-bar link mechanism
A mechanism is divided into several units and each of them will be analyzed with systematic calculation
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
Feature of the systematic kinematics analysis(1)It is easy to understand because motion of kinematic
pair can be calculated in order(2)Applicable to various planar mechanisms with
revolute and prismatic pairs(3)Velocity and acceleration can be calculated(4)Fundamentals to analyze dynamics of mechanism
ldquoGeneral systematic and available methodrdquo
rarrCalculation program can be offered
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
41 Derivation of systematic kinematic analysis (1)Motion of crank linkcrank_input
++++
=
0
0
1
1
)sin()cos(
YLXL
YX
φθφθ
To consider the postureof front link
++++++minus
=
0
0
1
1
)cos()()sin()(
YLXL
YX
φθφθφθφθ
+++minus++++++++minus
=
02
02
1
1
)]sin()()cos()[()]cos()()sin()[(
YLXL
YX
φθφθφθφθφθφθφθφθ
Differentiating
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φ
θL
Crank link
Input parameters
Output parameters
Mechanism constants
Position and posture of pair J0 rarrCrank angle rarrPosition of pair J1
(X0Y0)φ θ (X1Y1)
From a pair to a pairVelocity and acceleration
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
(2)Motion of input sliderslider_input
++
=
L
L
S
S
YsXs
YX
ψψ
sincos
+++minus
=
L
L
S
S
YssXss
YX
ψψψψψψ
cossinsincos
+minus+++minusminusminus
=
L
L
S
S
YssssXssss
YX
ψψψψψψψψψψψψψψ
sincoscos2sincossinsin2cos
2
2
+minus+minusminus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψψψψψ
sincoscossin
++minusminus+minusminus+minus
=
SCC
SCC
C
C
YyxXyx
YX
)cossin()sincos()sincos()cossin(
22
22
ψψψψψψψψψψψψψψψψ
Position and posture of slider axisrarrTranslational input rarrPosition of pair JC
(XLYL)ψ s (XCYC)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
Input slider link
Posture of slider axis
+++minus
=
SCC
SCC
C
C
YyxXyx
YX
ψψψψ
cossinsincos Position of pair JC
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
φL
(3)Angular motion of linklink_angle
XY
∆∆1tanminus=φ
2LXYXY
∆∆∆∆ minus=φ
2LXYXY
∆∆∆∆ minus=φ
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Posture angleIs represented with positionof pairs
Position of pairs at both ends rarr Posture of link(X0 Y0) (X1 Y1) φ
Angular motion of link
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
O X
Y
X
J1( X1 Y1 )
J0 ( X0 Y0 )
αL
(4)Angular motion of segmentsegment_angle
Length varies
Segment connecting two points
XY
∆∆1tanminus=α Let consider
change of distance
αα 22 cos
XXYXY
∆∆∆∆∆
minus
=
3cossin)(2
cos)](2)([
XXXYXY
XYXYXXYXYX
∆sdot
∆∆∆minus∆∆minus
∆∆minus∆∆∆minus
∆∆minus∆∆∆=
α
αα
α
α
010101
010101
YYYYYYYYYXXXXXXXXX
minus=∆minus=∆minus=∆
minus=∆minus=∆minus=∆where
Position of two points rarr Posture of segment (X0 Y0) (X1 Y1) α
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
O X
Y
X
J1 ( X1 Y1 )
J0 ( X0 Y0 )
JC ( XC YC ) xy
yCxCφ
(5)Motion of coupler pointcoupler_point
Point on a link
+++minus
=
0
0
cossinsincos
YyxXyx
YX
CC
CC
C
C
φφφφ
+minus++minus
=
0
0
)sincos()cossin(
YyxXyx
YX
CC
CC
C
C
φφφφφφ
++minusminus+minusminus+minus
=
02
02
)cossin()sincos()sincos()cossin(
YyxyxXyxyx
YX
CCCC
CCCC
C
C
φφφφφφφφφφφφ
φ can be calculated with link_angle
Position of pairs J0J1 rarr Position of pair JC(X0Y0) (X1Y1 ) (XCYC)
Coordinate transformation due to rotation
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
(6)Motion of two adjacent links with only revolute pairsRRR_links
++
=
000
000
sincos
YLXL
YX
B
B
φφ
Two adjacent links with only revolute pair
By giving posture angle φ0 position of pair JB can be calculated
J1( X1 Y1 )
O X
Y
XJ0 ( X0 Y0 )
L
αβ
β
L0
L1
L0
L1
JB ( XB YB )
JBrsquo
Φ0
φ0
+minus++minus
=
002
0000
002
0000
)sincos()cossin(
YLXL
YX
B
B
φφφφφφφφ
βαφβαφβαφ plusmn=plusmn=plusmn= 000 where
++minus
=
0000
0000
cossin
YLXL
YX
B
B
φφφφ
Calculated withsegment angle
22 YXL ∆∆ +=
From cosine theorem
LLLLL
0
21
2201
2cos minus+
= minusββ
βsin2
)(2
0
21
220
LLLLLL
minusminus=
)sin2(cos)(
sin]2)2)([(23
12
122
0
22221
220
βββ
ββ
LLLLLLL
LLLLLLLL
minusminusminus
minusminusminusminus=
By differentiating
Sign corresponds to mechanical inversion
Position of pairs J0J1rarrPosition of pair JB(X0Y0) (X1Y1 ) (XBYB)
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
O X
Y
XPL ( XL YL )
ψ
s
JC ( XC YC )
x
y
yC
xC
JR ( XR YR )L
Two adjacent links with prismatic and revolute pairs
P Displacement of sliderDxBAs C plusmnminusminusminus= ψψ sincos
where
RLC YYyB minus+= ψcosRLC XXyA minus+minus= ψsin
22 )cossin( ψψ BALD minusminus=where
DDBBAAs
2cossinsincos
plusmnminusminus+minus= ψψψψψψ
DDDDDBBBB
AAAAs
42sincoscos2sin
cossinsin2cos2
2
2
minusplusmn+minusminusminus
+++minus=
ψψψψψψψ
ψψψψψψψ
)cossin)(sincoscossin(2 ψψψψψψψψ BABBAAD minus+minus+minus=
RLC XXyA minus+minus= ψψ cos RLC YYyB minus+minus= ψψ sin hellipVelocity and acceleration can be calculated with slider_input
Position and posture ofslider axis position of pair JR(XLYL)ψ(XRYR)
Position of pair JCDisplacement of slider
(XCYC)s
++++minus+
=
LCC
LCC
C
C
YyxsXyxs
YX
ψψψψ
cossin)(sincos)(
222 )()( LYYXX RCRC =minus+minus
Pair JC is located at the intersectionbetween straight line P and a circle with radius L
(7)Motion of two adjacent links with prismatic and revolute pairsPRR_links
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
C language program of systematic kinematics analysis
------------------------------------------------------------ Kinematics Calculation lt mechcpp gt 2002 814 Created by D Matsuuura 20031013 Improved by D Matusuura 2008 112 Improved by D Matsuuura 2008 114 Improved by N Iwatsuki 2010 821 Improved by N Iwatsuki 20111010 Improved by N Iwatsuki 2018 415 Improved by N Iwatsuki------------------------------------------------------------
include ltstdafxhgtinclude ltstdiohgtinclude ltmathhgtinclude mechhinclude matrixh
----------------------------------------------------------- Input slider motion pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide s Displacement velocity and acceleration of an input slider gsi eta Moving coordinate of a pair on a slider jp Displacement velocity and acceleration of a slider jc Displacement velocity and acceleration of a pair on a slider-----------------------------------------------------------
void crank_input( POINT j1 double l ANGLE theta ANGLE phi POINT j2 )
double thephi dthephi ddthephidouble cs
thephi = thetathe + phithedthephi = thetadthe + phidtheddthephi = thetaddthe + phiddthe
c = cos( thephi )s = sin( thephi )
j2-gtPx = j1Px + lcj2-gtDPx = -lsdthephij2-gtDDPx = -l( sddthephi + cdthephidthephi )
j2-gtPy = j1Py + lsj2-gtDPy = lcdthephij2-gtDDPy = l( cddthephi - sdthephidthephi )
----------------------------------------------------------- Input crank motion j1 Displacement velocity and acceleration of root pair l Link length theta Angular displacement velocity and acceleration of crank phi Angular displacement velocity and acceleration of root link j2 Displacement velocity and acceleration of tip pair-----------------------------------------------------------
void slider_input( POINT pg double l ANGLE psig VARIABLE sdouble gsi double eta POINT jp POINT jc )
double cp sp
cp = cos( psigthe )sp = sin( psigthe )
jp-gtPx = svcp + pgPxjp-gtDPx = sdvcp - svsppsigdthe + pgDPxjp-gtDDPx = sddvcp - 20sdvsppsigdthe - svsppsigddthe
- svcppsigdthepsigdthe + pgDDPx
jp-gtPy = svsp + pgPyjp-gtDPy = sdvsp + svcppsigdthe + pgDPyjp-gtDDPy = sddvsp + 20sdvcppsigdthe + svcppsigddthe
- svsppsigdthepsigdthe + pgDDPy
jc-gtPx = gsicp - etasp + jp-gtPxjc-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjc-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )
- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jc-gtPy = gsisp + etacp + jp-gtPyjc-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPyjc-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe )
- eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
----------------------------------------------------------- Angular motion of a segment connecting two points j1 j2 Displacement velocity and acceleration of each point angle Angular displacement velocity and acceleration of a segment connecting two points Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int segment_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble c sdouble absx absydouble q1 q2double r dr
get_diff( j1 j2 ampdiff )absx = fabs( diffPx )absy = fabs( diffPy )if( absx lt 10e-20 ampamp absy lt 10e-20 )
angle-gtthe = 00angle-gtdthe = 00angle-gtddthe = 00printf(ldquo++ Error in segment_angle yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )
c = cos( angle-gtthe )s = sin( angle-gtthe )
if( absy gt 10e-20 ampamp absxabsy lt 10e-10 )r = sqrt( diffPxdiffPx + diffPydiffPy )dr = ( diffDPxdiffPx + diffDPydiffPy )rif( s gt= 00 )
angle-gtdthe = -diffDPxrangle-gtddthe = ( -diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
angle-gtdthe = diffDPxrangle-gtddthe = ( diffDDPx - 20drangle-gtdthe )rreturn SUCCESS
else
q1 = diffDPydiffPx - diffPydiffDPxq2 = diffDDPydiffPx - diffPydiffDDPxangle-gtdthe = q1cc(diffPxdiffPx)angle-gtddthe = ( (diffPxq2 - 20diffDPxq1)c - 20diffPxangle-gtdtheq1s)c( diffPxdiffPxdiffPx )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links Connecting Revolute Pairs j1 j2 Displacement velocity and acceleration of pairs at both ends l1 l2 Link lengths minv Indicator for mechanical inversion jb Displacement velocity and acceleration of a pair which connects two links Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int RRR_links( POINT j1 POINT j2 double l1 double l2 int minv POINT jb )
POINT diffdouble q2double q dq ddq
double cb sb ssANGLE angledouble phi dphi ddphidouble bet dbet ddbetdouble cp spint ier
get_diff( j1 j2 ampdiff )q2 = diffPxdiffPx + diffPydiffPy
if( q2 lt= 10e-30 )printf(++ Error in RRR_links (2対偶が一致してます) yenn)return ERROR
q = sqrt( q2 )dq = ( diffDPxdiffPx + diffDPydiffPy )qddq = ( ( diffDDPxdiffPx + diffDPxdiffDPx
+ diffDDPydiffPy + diffDPydiffDPy )q- ( diffDPxdiffPx + diffDPydiffPy )dq )q2
cb = ( l1l1 + q2 - l2l2 )( 20l1q )if( fabs( cb ) gt 10 )
printf(ldquo++ Error in RRR_links (連鎖が構成できません)yennrdquo)return ERROR
sb = sqrt( 10 - cbcb )ss = l1l1 - l2l2 - q2
ier = segment_angle( j1 j2 ampangle )if( ier = SUCCESS )
printf(ldquo++ Error in RRR_links (2対偶が一致してます) yennrdquo)return ERROR
bet = acos( cb )
dbet = ssdq( 20l1q2sb )
C language program of systematic kinematics analysis
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
ddbet = ( ( ss( ddqq - 20dqdq ) - 20( dqq )( dqq) )sb - ssdqqdbetcb )( 20l1q2qsbsb )
if( minv gt 0 ) phi = anglethe + betdphi = angledthe + dbetddphi = angleddthe + ddbet
else
phi = anglethe - betdphi = angledthe - dbetddphi = angleddthe - ddbet
cp = cos( phi )sp = sin( phi )
jb-gtPx = j1Px + l1cpjb-gtPy = j1Py + l1spjb-gtDPx = j1DPx - l1dphispjb-gtDPy = j1DPy + l1dphicpjb-gtDDPx = j1DDPx - l1( ddphisp + dphidphicp )jb-gtDDPy = j1DDPy + l1( ddphicp - dphidphisp )
return SUCCESS
----------------------------------------------------------- Motion of Adjacent Two links with a Prismatic Pair and Two Revolute Pairs jr Displacement velocity and acceleration of revolute pair at end l Link legth between two revolute pairs pg Displacement velocity and acceleration of a point on a linear guide to describe position of the linear guide psig Angular displacement velocity and acceleration of linear guide gsi eta Moving coordinate of a pair on a slider minv Indicator for mechanical inversion s Displacement velocity and acceleration of a slider on moving coordinate system jp Displacement velocity and acceleration of a slider on fixed coordinate system jb Displacement velocity and acceleration of a revolute pair connecting two linksr Function value Completeincomplete(Link chain cannot be formed)-----------------------------------------------------------
int PRR_links( POINT jr double l POINT pg ANGLE psig double gsi double etaint minv VARIABLE s POINT jp POINT jb )
double cp spdouble a da dda b db ddb d dd ddd q dq ddq s0 ds0 dds0
cp = cos( psigthe )sp = sin( psigthe )
a = -etasp + pgPx - jrPxb = etacp + pgPy - jrPy
d = ll - ( asp - bcp )( asp - bcp )if( dlt00 ) return ERROR
da = -etacppsigdthe + pgDPx - jrDPxdda = -eta( cppsigddthe - sppsigdthepsigdthe ) + pgDDPx - jrDDPx
db = -etasppsigdthe + pgDPy - jrDPyddb = -eta( sppsigddthe + cppsigdthepsigdthe ) + pgDDPy - jrDDPy
dd = -20( dasp + acppsigdthe - dbcp + bsppsigdthe )( asp - bcp )ddd = -20( ( ddasp + 20dacppsigdthe + acppsigddthe
- asppsigdthepsigdthe- ddbcp + 20dbsppsigdthe + bsppsigddthe+ bcppsigdthepsigdthe )( asp - bcp ) + ( dasp + acppsigdthe - dbcp + bsppsigdthe )
( dasp + acppsigdthe - dbcp + bsppsigdthe ) )
q = sqrt( d )dq = dd( 20q )ddq = ( 20dddd - dddd )( 40dq )
s0 = -acp - bsp - gsids0 = -dacp + asppsigdthe - dbsp - bcppsigdtheC language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- 節の角変位(節上の2対偶間を結ぶ線分の成す角) j1 j2 位置ベクトルとその微分 angle 線分の成す角 関数値 0 正常終了 1 異常終了(2点が一致)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
dds0 = -ddacp + 20dasppsigdthe + asppsigddthe+ acppsigdthepsigdthe - ddbsp - 20dbcppsigdthe - bcppsigddthe + bsppsigdthepsigdthe
if( minvgt0 ) s-gtv = s0 + qs-gtdv = ds0 + dqs-gtddv = dds0 + ddq
else
s-gtv = s0 - qs-gtdv = ds0 - dqs-gtddv = dds0 - ddq
jp-gtPx = s-gtvcp + pgPxjp-gtPy = s-gtvsp + pgPy
jp-gtDPx = s-gtdvcp - s-gtvsppsigdthe + pgDPxjp-gtDPy = s-gtdvsp + s-gtvcppsigdthe + pgDPy
jp-gtDDPx = s-gtddvcp - 20s-gtdvsppsigdthe - s-gtvsppsigddthe - s-gtvcppsigdthepsigdthe + pgDDPx
jp-gtDDPy = s-gtddvsp + 20s-gtdvcppsigdthe + s-gtvcppsigddthe - s-gtvsppsigdthepsigdthe + pgDDPy
jb-gtPx = gsicp - etasp + jp-gtPxjb-gtPy = gsisp + etacp + jp-gtPy
jb-gtDPx = -gsisppsigdthe - etacppsigdthe + jp-gtDPxjb-gtDPy = gsicppsigdthe - etasppsigdthe + jp-gtDPy
jb-gtDDPx = -gsi( sppsigddthe + cppsigdthepsigdthe )- eta( cppsigddthe - sppsigdthepsigdthe ) + jp-gtDDPx
jb-gtDDPy = gsi( cppsigddthe - sppsigdthepsigdthe ) - eta( sppsigddthe + cppsigdthepsigdthe ) + jp-gtDDPy
return SUCCESS
----------------------------------------------------------- Angular Motion of Link j1 j2 Displacement velocity and acceleration of two points on a link angle Angular displacement velocity and acceleration of link Function value Completeincomplete(Two points are quite same)-----------------------------------------------------------
int link_angle( POINT j1 POINT j2 ANGLE angle )
POINT diffdouble q
get_diff( j1 j2 ampdiff )
q = diffPxdiffPx + diffPydiffPyif( q lt= 10e-30 )
printf(ldquo++ Error in link_angle (2対偶が一致してます)yennrdquo)return ERROR
angle-gtthe = atan2( diffPy diffPx )angle-gtdthe = ( diffDPydiffPx - diffPydiffDPx )qangle-gtddthe = ( diffDDPydiffPx - diffPydiffDDPx )q
return SUCCESS
C language program of systematic kinematics analysis
You can download these programs fromWEB-sitehttpwwwdynamicsmeptitechacjpjapanesedownloadhtml
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
(1)Give position of fixed pairsGive position of fixed revolute pairs and position and posture of fixed prismatic pairs
(3)Calculate motion of moving pair oninput links
By using coupler_point motion of other moving pairs on input links can be calculated
(2)Calculate motion of input linksBy using crank_input or slider_input motion of input link can be calculated
(4)Calculate motion of two adjacent linksBy using RRR_links or PRR_links motion of two adjacent links with revoluteprismatic pair can becalculated
(5)Calculate motion of pairs on coupler linksBy using coupler_point motion of moving pairs on coupler link can be calculated(6)Repeat (4) and (5) and then calculate all moving pairs
END
Ex Crank input-Slider outputplanar 6-bar link mechanism
ldquoIt is important to find two adjacentlinks to be analyzedrdquo
42 Procedure of the systematic kinematics analysis
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Examples of analysis(1)Crank input-Slider output planar 6-bar link mechanism
(0000)
(0000)
(0800)
03
0805
(0302)(0002) 08
135deg
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
X
Y
O
J1J6
J2
J0 ( X0 0 )J3( X3 0 )
J4 ( X4 Y4 )
J5 ( 0 yC5 )
y2
x2
xC4
yC4
θψ
L0
L1L2
L3
s
Coupler point coupler_point
Two adjacent linkswith only revolute pairsRRR_links
Crank input linkcrank_input
Two adjacent links with prismaticrevolute pairsPRR_links
1)Crank input-Slider output planar 6-bar link mechanismProcedure hellip
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
Locus of J4 of the 6-bar mechanism
Y 4m
X4 m
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
Displacement velocity and acceleration of output slider of the 6-bar mechanism
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
(2)A planar closed-loop manipulator with 2 DOF
Forward kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
Output motion
J1
J0J4
J3
J2 Crank input linkcrank_input
Two adjacent links with only revolute pairsRRR_links
Moving point on coupler linkcoupler_point
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
Inverse kinematics
θ1
r
X
Y
O
θ2 L0L3
L2
L1
J1
J0J4
J3
J2 Two adjacent links with only revolute pairsRRR_links
Moving point on coupler link
coupler_pointTwo adjacent links with only revolute pairsRRR_links
All motion can easilybe calculated
Input angle link_angle
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
X
X
X
X
X
Y
O
xy
φ
θ0
θ1
θ2
L0L1
L0
L1
L0
L1
J0
J1
J2
J3
J4
J5J6
J7
J8
P
By specifying the position and motion of output platform
0606 crank_input
coupler_pointRRR_links
link_angle
Angular displacement of input link can be calculated
(3)Inverse kinematics of a planar closed-loop manipulator with 3 DOF
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
O X
Y
X
( X Y )
θ1
L1
θ2L2
M
M
(X0Y0)
(4)Planar serial 2R manipulator
J0
J1
J2
Serially connected twocrank input linkcrank_input
Only crank_input
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
XJ0 (X0Y0=(18 03)
Y
O
J3
J4
J1(X1Y1)=(10 02 )
J5 ( 02 04)
J6
η0
θ
φ
04
07
J2(X2Y2)=(05 03)
ξ0
0710
J7 ( 04 05)
ξ0
η1
J4
10
L0
L1
L2L3
L4
ξ1
08
L5
(4)Planar 6-bar link mechanism type-E
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
XJ0 (X0Y0=(12 00)
Y
O
J3
J4
J1(X1Y1)=(03 02 )
J5 ( 06 06)J6
η0
θ
φ
05
07
J2(X2Y2)=(00 18)
ξ0
04
10
J7 ( 04 -07)
ξ1
η1
J4
10
L0
L1L2
L3
L4
(5)Planar 6-bar link mechanism type-B
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
XJ0 (X0Y0)=(05 00)
Y
O
J2
J4
J5 ( 00 10)
J6 η0
θ
φ
03
04
05
ξ0
J7 ( 10 10)
ξ1
η1
J3
05
η2
ξ2
J4 (00 05)
14
L0
L1L2
J1 (X1Y1)=(00 00)
L4
L3
(6)Planar 6-bar link mechanism type-F
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
(7)Planar closed-loop leg-mechanism with 1 DOF
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
2 legs with only 1 DOF
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
(8)Planar closed-loop leg-mechanism with 1 DOFldquoTheo Jansenrsquos mechanismrdquo
Planar 8-bar mechanism with only 1 DOF
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
2 legs with only 1 DOF
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
A quadruped walking machineswith only 1 DOF
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
Slippage between 2 legsdarr
It can walk on sand beach
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
5 Concluding remarksAs an available tool to analyze or design link mechanisms is proposed formulated and coded(1)Various planar mechanisms can be systematically
analyzed(2)Not only displacement but also velocity and
acceleration can be calculated and will be applied to dynamics calculation
(3)Students are expected to experience to analyze some mechanisms with the offered programs
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-
Homework 1Calculate time history of output displacementvelocity and acceleration of some planar linkmechanism by using the offered program orself-coded program It is OK to use the offered program as is
The result will be summarized in A4 size PDFwith less than 5 pages and sent to Prof Iwatsukivia OCW-i by July 6 2020
Of course students should describe comments for the calculated results
- スライド番号 1
- スライド番号 2
- スライド番号 3
- スライド番号 4
- スライド番号 5
- スライド番号 6
- スライド番号 7
- スライド番号 8
- スライド番号 9
- スライド番号 10
- スライド番号 11
- スライド番号 12
- スライド番号 13
- スライド番号 14
- スライド番号 15
- スライド番号 16
- スライド番号 17
- スライド番号 18
- スライド番号 19
- スライド番号 20
- スライド番号 21
- スライド番号 22
- スライド番号 23
- スライド番号 24
- スライド番号 25
- スライド番号 26
- スライド番号 27
- スライド番号 28
- スライド番号 29
- スライド番号 30
- スライド番号 31
- スライド番号 32
- スライド番号 33
- スライド番号 34
- スライド番号 35
- スライド番号 36
- スライド番号 37
- スライド番号 38
- スライド番号 39
- スライド番号 40
- スライド番号 41
- スライド番号 42
- スライド番号 43
- スライド番号 44
- スライド番号 45
- スライド番号 46
- スライド番号 47
- スライド番号 48
- スライド番号 49
- スライド番号 50
- スライド番号 51
- スライド番号 52
- スライド番号 53
- スライド番号 54
- スライド番号 55
- スライド番号 56
- スライド番号 57
- スライド番号 58
- スライド番号 59
- スライド番号 60
- スライド番号 61
- スライド番号 62
- スライド番号 63
- スライド番号 64
- スライド番号 65
- スライド番号 66
- スライド番号 67
- スライド番号 68
- スライド番号 69
- スライド番号 70
- スライド番号 71
- スライド番号 72
- スライド番号 73
- スライド番号 74
- スライド番号 75
- スライド番号 76
- スライド番号 77
- スライド番号 78
- スライド番号 79
- スライド番号 80
- スライド番号 81
- スライド番号 82
-