107 lines
2.3 KiB
Python
107 lines
2.3 KiB
Python
# pick up position
|
|
startv = Pose()
|
|
startv.position.x = 0.474
|
|
startv.position.y = 0.708
|
|
startv.position.z = 0.254
|
|
startv.orientation.x = 0.646
|
|
startv.orientation.y = 0.762
|
|
startv.orientation.z = -0.003
|
|
startv.orientation.w = 0.003
|
|
|
|
brick1 = Pose()
|
|
brick1.position.x = 0.615
|
|
brick1.position.y = 0.194
|
|
brick1.position.z = -0.131
|
|
brick1.orientation.x: 0.656
|
|
brick1.orientation.y: 0.753
|
|
brick1.orientation.z: -0.013
|
|
brick1.orientation.w: -0.015
|
|
|
|
brick2 = Pose()
|
|
brick2.position.x = 0.615
|
|
brick2.position.y = 0.346
|
|
brick2.position.z = -0.125
|
|
brick2.orientation.x: 0.667
|
|
brick2.orientation.y: 0.742
|
|
brick2.orientation.z: -0.045
|
|
brick2.orientation.w: 0.033
|
|
|
|
brick3 = Pose()
|
|
brick3.position.x = 0.615
|
|
brick3.position.y = 0.532
|
|
brick3.position.z = -0.122
|
|
brick3.orientation.x: 0.689
|
|
brick3.orientation.y: 0.724
|
|
brick3.orientation.z: 0.008
|
|
brick3.orientation.w: 0.014
|
|
|
|
##horizontal 1
|
|
#pick up position
|
|
starth = Pose()
|
|
starth.position.x = 0.464
|
|
starth.position.y = 0.804
|
|
starth.position.z = 0.116
|
|
starth.orientation.x: 0.661
|
|
starth.orientation.y: 0.748
|
|
starth.orientation.z: 0.043
|
|
starth.orientation.w: -0.006
|
|
|
|
brick4 = Pose()
|
|
brick4.position.x = 0.615
|
|
brick4.position.y = 0.240
|
|
brick4.position.z = -0.0499
|
|
brick4.orientation.x: 0.692
|
|
brick4.orientation.y: 0.720
|
|
brick4.orientation.z: -0.030
|
|
brick4.orientation.w: -0.021
|
|
|
|
|
|
brick5 = Pose()
|
|
brick5.position.x = 0.615
|
|
brick5.position.y = 0.449
|
|
brick5.position.z = -0.058
|
|
brick5.orientation.x: 0.677
|
|
brick5.orientation.y: 0.734
|
|
brick5.orientation.z: -0.032
|
|
brick5.orientation.w: -0.016
|
|
|
|
brick6 = Pose()
|
|
brick6.position.x = 0.615
|
|
brick6.position.y = 0.247
|
|
brick6.position.z = 0.123
|
|
brick6.orientation.x: 0.656
|
|
brick6.orientation.y: 0.753
|
|
brick6.orientation.z: -0.040
|
|
brick6.orientation.w: 0.000
|
|
|
|
brick7 = Pose()
|
|
brick7.position.x = 0.615
|
|
brick7.position.y = 0.441
|
|
brick7.position.z = 0.138
|
|
brick7.orientation.x: 0.671
|
|
brick7.orientation.y: 0.735
|
|
brick7.orientation.z: -0.060
|
|
brick7.orientation.w: 0.005
|
|
|
|
##horizontal 2
|
|
brick8 = Pose()
|
|
brick8.position.x = 0.635
|
|
brick8.position.y = 0.334
|
|
brick8.position.z = 0.209
|
|
brick8.orientation.x: 0.680
|
|
brick8.orientation.y: 0.729
|
|
brick8.orientation.z: -0.048
|
|
brick8.orientation.w: -0.042
|
|
|
|
brick9 = Pose()
|
|
brick9.position.x = 0.635
|
|
brick9.position.y = 0.349
|
|
brick9.position.z = 0.406
|
|
brick9.orientation.x: 0.674
|
|
brick9.orientation.y: 0.737
|
|
brick9.orientation.z: -0.008
|
|
brick9.orientation.w: 0.016
|
|
|
|
|
|
|