Skip to content

Commit a7c7ef4

Browse files
author
OSSome01
committed
dummy joint added
1 parent f70e0bd commit a7c7ef4

File tree

2 files changed

+14
-10
lines changed

2 files changed

+14
-10
lines changed

Scripts/broadcaster.py

Lines changed: 13 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,8 @@
3636
new_theta3 = 0
3737
alpha3 = math.pi/2
3838
new_alpha3 = 0
39+
dummy_a3 = 0.1
40+
new_dummy_a3 = 0
3941
link3_orientation = tf.transformations.quaternion_from_euler(alpha3, 0, theta3)
4042

4143
#DH parameters for link 4
@@ -48,21 +50,21 @@
4850
#Transform data is sent
4951
link1_to_base.sendTransform((0, 0, d1), link1_orientation, rospy.Time.now(), "link1", "base")
5052
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")
5254
link4_to_link3.sendTransform((0, 0, d4), link4_orientation, rospy.Time.now(), "link4", "link3")
5355

5456
# 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
5859
rospy.sleep(0.1)
59-
print("new_alpha1 updated", new_alpha1)
60+
print("new_d1 updated", new_d1)
6061
new_link1_orientation = tf.transformations.quaternion_from_euler(new_alpha1, 0, 0)
6162
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
6466
rospy.sleep(0.1)
65-
print("new_d1 updated", new_d1)
67+
print("new_alpha1 updated", new_alpha1)
6668
new_link1_orientation = tf.transformations.quaternion_from_euler(new_alpha1, 0, 0)
6769
mobile1_to_link1.sendTransform((0, 0, new_d1), new_link1_orientation, rospy.Time.now(), "mobile1", "base")
6870

@@ -86,8 +88,9 @@
8688
print("new_theta3 updated", new_theta3)
8789
if new_alpha3 < alpha3 and new_theta3 >= theta3:
8890
new_alpha3 = new_alpha3 + 0.015
91+
new_dummy_a3 = new_dummy_a3 + 0.00095493
8992
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")
9194
rospy.sleep(0.1)
9295
print("new_alpha3 updated", new_alpha3)
9396

@@ -107,6 +110,7 @@
107110
new_theta3 = 0
108111
new_alpha3 = 0
109112
new_d4 = 0
113+
new_dummy_a3 = 0
110114

111115
except:
112116
continue
Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
<launch>
22
<!-- Load the urdf into the parameter server. -->
33
<node name="tf2_broadcaster" pkg="dh_transformation_visualization" type="broadcaster.py" respawn="false" output="screen" />
4-
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find DH_transformation_visualization)/config/rviz_config.rviz"/>
4+
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find dh_transformation_visualization)/config/rviz_config.rviz"/>
55
</launch>
66

0 commit comments

Comments
 (0)