GAZEBO
=
## ==OUTLINE==
### 1.Use blender transform .stl to .dae
### 2.Import (visual) mesh into URDF
### 3.Create (collision) mesh file and import to URDF
### 4.Computing the inertial parameters with meshlab
### 5.Write a python program
### 6.Forward Kinematic Analysis
## ==1.Use blender transform .stl to .dae==
(1)open blender,並刪除不相干的物件以免影響輸出
(2)import 所有的 .stl 檔案,and Scale調整成0.1
(3)選取該Link的所有物件,並按Ctrl+j將其組成成一個群組
(4)調整origin,Object>Set origin>Origin to center of mass(volume)
並且調整Scale x,y,z為0.1,並輸出成(.dae)檔
## ==2.Import (visual) mesh into URDF==
(1)將輸出的.dae檔import到.xarco裏面
```xml=
<visual>
<origin xyz="0 0 -0.73" rpy="0 0 0"/>
<geometry>
<mesh filename="package://rrbot_description/meshes/link_1.dae"/>
</geometry>
<material name="black"/>
</visual>
```
(2)修改origin xyz和rpy
在blender中我們觀察Location的部份,下圖為兩個鄰近的link的座標位置,我們可以透過這兩張圖知道其相對位置,並將兩個數值相減就是origin xyz的數值,要注意的是輸入blender跟輸出時我們有經過rotation,因此URDF中的origin xyz可能跟blender的xyz有所不同
rpy的部份我們再blender已經經過rotation,因此rpy都設為0
 
(3)Gazebo
## ==3.Create (collision) mesh file and import to URDF==
建立碰撞屬性的mesh有很多方法,最簡單的方式是直接套用前面visual的mesh,但**過於複雜的模型會導致花費大量時間及效能在運算上**,因此我們利用一些簡單的幾何模型來建立出我們的collision mesh,來減少運算的時間
(1)Blender量測參數
我們先利用 blender的measure量測出模型的相關參數並紀錄下來
(2)THINKERCAD建立collision mesh
依照量測的參數,利用線上的3D繪圖軟體來建立出簡單的幾何模型,並輸出成stl檔
(3)將輸出的stl檔案import到.xarco裡
```xml=
<collision>
<origin xyz="0 0 -0.73" rpy="0 0 0"/>
<geometry>
<mesh filename="package://rrbot_description/meshes/link_1.stl"/>
</geometry>
</collision>
```
(4)Gazebo
## ==4.Computing the inertial parameters with meshlab==
(1)開啟meshlab,並將預計算模型載入
(2)計算模型慣性
View->Show Layer Dialog
Filters->Quality Measure and Computations->Compute Geometric Measures
```python=
Opened mesh /home/yue/catkin_ws/src/gazebo_ros_demos/rrbot_description/meshes/4.stl in 146 msec
All files opened in 1813 msec
OUT OF SCOPE
Mesh Bounding Box Size 1.710000 0.760000 0.760000
Mesh Bounding Box Diag 2.019728
Mesh Bounding Box min -0.972894 -0.508655 -0.385242
Mesh Bounding Box max 0.737106 0.251345 0.374758
Mesh Surface Area is 4.621989
Mesh Total Len of 234 Edges is 98.149529 Avg Len 0.419442
Mesh Total Len of 234 Edges is 98.149529 Avg Len 0.419442 (including faux edges))
Thin shell (faces) barycenter: -0.206732 -0.059588 -0.001047
Vertices barycenter -0.061459 -0.026841 -0.005242
Mesh Volume is 0.576611
Center of Mass is -0.266000 -0.066500 0.000000
Inertia Tensor is :
| 0.041605 -0.020839 -0.000548 |
| -0.020839 0.142427 0.000525 |
| -0.000548 0.000525 0.142409 |
Principal axes are :
| 0.980851 0.194714 0.004144 |
| -0.024198 0.142953 -0.989434 |
| -0.193249 0.970387 0.144927 |
axis momenta are :
| 0.037466 0.142319 0.146655 |
Applied filter Compute Geometric Measures in 167 msec
```
(3)修改URDF中的慣性參數
根據上面計算出來的結果我們可以得知慣性的參數,並填入到.xarco檔
```
Inertia Tensor is :
| 0.041605 -0.020839 -0.000548| ==> | ixx ixy ixz |
| -0.020839 0.142427 0.000525 | ==> | ixy iyy iyz |
| -0.000548 0.000525 0.142409 | ==> | ixz iyz izz |
```
(4)Gazebo
粉紅色部份為計算出來的慣性
## ==5.Write a python program==
### 1.利用輸入角度來控制手臂移動
(1)Code
```python=
#!/usr/bin/env python
import rospy
import math
from std_msgs.msg import Float64
from math import sin,cos,atan2,sqrt,fabs
from std_msgs.msg import String
import time
deg=0.017453293
#Define a RRBot joint positions publisher for joint controllers.
def rrbot_joint_positions_publisher():
#Initiate node for controlling joint1 and joint2 positions.
rospy.init_node('joint_positions_node', anonymous=True)
#Define publishers for each joint position controller commands.
pub1 = rospy.Publisher('/rrbot/joint1_position_controller/command', Float64, queue_size=20)
pub2 = rospy.Publisher('/rrbot/joint2_position_controller/command', Float64, queue_size=20)
pub3 = rospy.Publisher('/rrbot/joint3_position_controller/command', Float64, queue_size=20)
pub4 = rospy.Publisher('/rrbot/joint4_position_controller/command', Float64, queue_size=20)
pub5 = rospy.Publisher('/rrbot/joint5_position_controller/command', Float64, queue_size=20)
pubb = rospy.Publisher('/rrbot/jointb_position_controller/command', Float64, queue_size=20)
rate = rospy.Rate(100) #100 Hz
while not rospy.is_shutdown():
angleb = input("Input your jointb angle(180~-180):")
angle1 = input("Input your joint1 angle(90~-45):")
angle2 = input("Input your joint2 angle(90~-180):")
angle3 = input("Input your joint3 angle(180~-180):")
angle4 = input("Input your joint4 angle(100~-100):")
positionb = angleb*deg
position1 = angle1*deg
position2 = angle2*deg
position3 = angle3*deg
position4 = angle4*deg
pubb.publish(positionb)
pub1.publish(position1)
pub2.publish(position2)
pub3.publish(position3)
pub4.publish(position4)
rate.sleep() #sleep for rest of rospy.Rate(100)
print("Please Wait Move")
time.sleep(3)
cs=input("Continue or Shutdowm?(1 or 0):")
if cs==0:
break
else:
continue
#Main section of code that will continuously run unless rospy receives interuption (ie CTRL+C)
if __name__ == '__main__':
try:
rrbot_joint_positions_publisher()
except rospy.ROSInterruptException:
pass
```
(2)Demo
{%youtube jAWjvaFShWs %}
### 2.動6個座標點
(1)Code
```python=
#!/usr/bin/env python
import rospy
import math
from std_msgs.msg import Float64
from math import sin,cos,atan2,sqrt,fabs
from std_msgs.msg import String
import time
import numpy
deg=57.295779513
#Initiate node for controlling joint1 and joint2 positions.
rospy.init_node('joint_positions_node', anonymous=True)
#Define publishers for each joint position controller commands.
pub1 = rospy.Publisher('/rrbot/joint1_position_controller/command', Float64, queue_size=20)
pub2 = rospy.Publisher('/rrbot/joint2_position_controller/command', Float64, queue_size=20)
pub3 = rospy.Publisher('/rrbot/joint3_position_controller/command', Float64, queue_size=20)
pub4 = rospy.Publisher('/rrbot/joint4_position_controller/command', Float64, queue_size=20)
pub5 = rospy.Publisher('/rrbot/joint5_position_controller/command', Float64, queue_size=20)
pubb = rospy.Publisher('/rrbot/jointb_position_controller/command', Float64, queue_size=20)
rate = rospy.Rate(100) #100 Hz
#Define a RRBot joint positions publisher for joint controllers.
def rrbot_joint_positions_publisher(p1,p2,p3,p4,pb):
pubb.publish(pb)
pub1.publish(p1)
pub2.publish(p2)
pub3.publish(p3)
pub4.publish(p4)
rospy.loginfo("Jb deg= %.2f,J1 deg= %.2f,J2 deg= %.2f,J3 deg= %.2f,J4 deg= %.2f",pb*deg,p1*deg,p2*deg,p3*deg,p4*deg)
#rate.sleep() #sleep for rest of rospy.Rate(100)
def position():
for i in numpy.arange(-1.57,3.14,1.57):
rrbot_joint_positions_publisher(0.35,0.53,0,0.67,i)
time.sleep(3)
rrbot_joint_positions_publisher(0.1,0.3,0,1.14,i)
time.sleep(3)
rrbot_joint_positions_publisher(0.76,-0.28,0,1.07,i)
time.sleep(3)
def init():
time.sleep(5)
rospy.loginfo("========initing========")
rrbot_joint_positions_publisher(0,0,0,0,0)
time.sleep(5)
#Main section of code that will continuously run unless rospy receives interuption (ie CTRL+C)
if __name__ == '__main__':
try:
init()
position()
init()
rospy.loginfo("finish")
except rospy.ROSInterruptException:
pass
```
(2)Demo
{%youtube k8sh_4vTBr0 %}
## ==6.Forward Kinematic Analysis==
(1)[參考網址](https://github.com/mkhuthir/RoboND-Kinematics-Project/tree/master/src)
(2)DH parameters
| Links | i | alpha(i-1) | a(i-1) | d(i) | theta(i) |
| ----- | --- |:----------:|:------:| ------ |:--------:|
| 0->1 | 1 | 0 | 0 | 0.73 | q1 |
| 1->2 | 2 | 270 | 0 | 1.081 | -90+q2 |
| 2->3 | 3 | 0 | 3.8 | -1 | q3 |
| 3->4 | 4 | -90 | 0 | 2.7397 | q4 |
| 4->5 | 5 | 90 | 0 | 0 | q5 |
| 5->6 | 6 | -90 | 0 | 1.2 | q6 |
| 6->7 | 7 | 0 | 0 | 0 | 0 |
(3)驗證
* q1~q6=0
* q1: 2.82, q2: -0.54, q3: -0.37, q4: -0.15, q5: -1.19, q6: 0
* q1: -1.64, q2: 0.63, q3: 0.22, q4: 2.85, q5: -0.85, q6: 0
驗證過後發現有些許誤差最大差距為0.03rad
Sign in with Wallet
Connect another wallet