|
36 | 36 | new_theta3 = 0
|
37 | 37 | alpha3 = math.pi/2
|
38 | 38 | new_alpha3 = 0
|
| 39 | + dummy_a3 = 0.1 |
| 40 | + new_dummy_a3 = 0 |
39 | 41 | link3_orientation = tf.transformations.quaternion_from_euler(alpha3, 0, theta3)
|
40 | 42 |
|
41 | 43 | #DH parameters for link 4
|
|
48 | 50 | #Transform data is sent
|
49 | 51 | link1_to_base.sendTransform((0, 0, d1), link1_orientation, rospy.Time.now(), "link1", "base")
|
50 | 52 | link2_to_link1.sendTransform((a2, 0, 0), link2_orientation, rospy.Time.now(), "link2", "link1")
|
51 |
| - link3_to_link2.sendTransform((0.2, 0, 0), link3_orientation, rospy.Time.now(), "link3", "link2") |
| 53 | + link3_to_link2.sendTransform((dummy_a3, 0, 0), link3_orientation, rospy.Time.now(), "link3", "link2") |
52 | 54 | link4_to_link3.sendTransform((0, 0, d4), link4_orientation, rospy.Time.now(), "link4", "link3")
|
53 | 55 |
|
54 | 56 | # Alingning base and link1
|
55 |
| - if new_alpha1 < alpha1: |
56 |
| - print("mobile1_to_link1") |
57 |
| - new_alpha1 = new_alpha1 + 0.015 |
| 57 | + if new_d1 < d1: |
| 58 | + new_d1 = new_d1 + 0.005 |
58 | 59 | rospy.sleep(0.1)
|
59 |
| - print("new_alpha1 updated", new_alpha1) |
| 60 | + print("new_d1 updated", new_d1) |
60 | 61 | new_link1_orientation = tf.transformations.quaternion_from_euler(new_alpha1, 0, 0)
|
61 | 62 | mobile1_to_link1.sendTransform((0, 0, new_d1), new_link1_orientation, rospy.Time.now(), "mobile1", "base")
|
62 |
| - if new_alpha1 >= alpha1 and new_d1 < d1: |
63 |
| - new_d1 = new_d1 + 0.005 |
| 63 | + if new_alpha1 < alpha1 and new_d1>= d1: |
| 64 | + print("mobile1_to_link1") |
| 65 | + new_alpha1 = new_alpha1 + 0.015 |
64 | 66 | rospy.sleep(0.1)
|
65 |
| - print("new_d1 updated", new_d1) |
| 67 | + print("new_alpha1 updated", new_alpha1) |
66 | 68 | new_link1_orientation = tf.transformations.quaternion_from_euler(new_alpha1, 0, 0)
|
67 | 69 | mobile1_to_link1.sendTransform((0, 0, new_d1), new_link1_orientation, rospy.Time.now(), "mobile1", "base")
|
68 | 70 |
|
|
86 | 88 | print("new_theta3 updated", new_theta3)
|
87 | 89 | if new_alpha3 < alpha3 and new_theta3 >= theta3:
|
88 | 90 | new_alpha3 = new_alpha3 + 0.015
|
| 91 | + new_dummy_a3 = new_dummy_a3 + 0.00095493 |
89 | 92 | new_link3_orientation = tf.transformations.quaternion_from_euler(new_alpha3, 0, new_theta3)
|
90 |
| - mobile3_to_link3.sendTransform((0, 0, 0), new_link3_orientation, rospy.Time.now(), "mobile3", "link2") |
| 93 | + mobile3_to_link3.sendTransform((new_dummy_a3, 0, 0), new_link3_orientation, rospy.Time.now(), "mobile3", "link2") |
91 | 94 | rospy.sleep(0.1)
|
92 | 95 | print("new_alpha3 updated", new_alpha3)
|
93 | 96 |
|
|
107 | 110 | new_theta3 = 0
|
108 | 111 | new_alpha3 = 0
|
109 | 112 | new_d4 = 0
|
| 113 | + new_dummy_a3 = 0 |
110 | 114 |
|
111 | 115 | except:
|
112 | 116 | continue
|
|
0 commit comments