Skip to content

Latest commit

 

History

History
106 lines (80 loc) · 3.62 KB

README.md

File metadata and controls

106 lines (80 loc) · 3.62 KB
Raw

HW3 Kinematic

จัดทำโดย

  • อสมา_6561
  • อิทธิกฤต_6562

Please install following library before run my code

To install, run the following command

Install sympy

pip install sympy

Install roboticstoolbox for python

pip install roboticstoolbox-python

Install numpy version 1.24.4

pip install numpy==1.24.4

Clone project

To Clone, run the following command

Clone to you directory

git clone https://github.com/Aitthikit/FRA333_HW3_6561_6562.git

You will see the file name

  • FRA333_HW3_6561_6562.py for Answer
  • testScript.py for Test
  • HW3_utils.py for Forward Kinematic function

Run Answers

python FRA333_HW3_6561_6562.py

you will see

alt text

  • Jacobian matrix reference to end-effector frame (6x3 Matrix)
  • Is it near singularity (1)
  • tau value [joint 1,joint 2,joint 3]

you can edit configulation space in this code at Input zone

alt text

This code is Calculation Jacobian Matrix from this Equation

alt text alt text

Run Test Scripts

python testScript.py

you will see

alt text

  • Focus at All True or False this testScript are check by use roboticstoolbox for python

This code is Calculation Jacobian Matrix from this DHparameter

alt text

you can edit configulation space in this code at Input zone same line as Run Answers

alt text

  • q is array of joint include with [ joint1, joint2, joint3 ]
  • w is wrench [ Fx, Fy, Fz, Mx, My, Mz ]
  • ref is reference frame ( 0 is base, 1 is end-effector )

Code 1

alt text

  • Receive the value q and ref to function
  • Receive and Calculate Rotation Matrix and Translation Matrix from HW3_utils
  • Create Rotation axis vector y specifying the direction of rotation of each joint ( z_1, z_2, z_3)

alt text

  • Calculate Jacobian Matrix by look at reference frame
  • Totol J_01, J_02, J_03 to Jacobian Matrix and return Matrix 6x3

Code 2

alt text

  • Get Jacobian Matrix from endEffectorJacobianHW3 function
  • Reduce Jacobian Matrix to Calculate determinant by using base.det
  • Check singularity by absolute value is less than 0.01 and return value ( if value is 1 = singularity, 0 is not singularity)

Code 3

alt text

  • Receive input value of q , w , ref
  • Get Jacobian Matrix from endEffectorJacobianHW3 function and Transpose to Calculate Force and Torque and add Force and Torque in the end-effector frame
  • Calculate Force
  • Calculate Torque with Jacobian
  • return joint Force and Torque as tau

Reference