Clean and comment code of the 1dof

ros2_ws/src/simple_robot_1dof/launch/simple_robot_rviz_simulation_1dof_launch.py

@@ -27,25 +27,25 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             'max_velocity',
-            default_value = '0.015',
+            default_value = '0.01725',
             )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
             'acceleration',
-            default_value = '0.005',
+            default_value = '0.001327',
             )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
             'decelaration',
-            default_value = '0.005',
+            default_value = '0.001327',
             )
     )
     declared_arguments.append(
         DeclareLaunchArgument(
             'velocity_constant',
-            default_value = '0.015',
+            default_value = '0.014',
             )
     )
     declared_arguments.append(

ros2_ws/src/simple_robot_1dof/simple_robot_1dof/forward_kinematics_node_1dof.py

@@ -18,13 +18,13 @@ class Forward_Kinematics_Node_1Dof(Node):
 
     def __init__(self):
         super().__init__('forward_kinematics_node_1dof')
-        # Create a subscriber, initialising the transformation
         self.subscription = self.create_subscription(
             Float64,
             '/dof1/reference_angles',
             self.listener_callback,
             10)
         
+        # load KUKA urdf
         urdf_file_path = os.path.join(
             get_package_share_directory('iiwa_description'), 'urdf', 'iiwa14.urdf')
         self.robot_model = loadURDF(urdf_file_path)
@@ -34,6 +34,7 @@ class Forward_Kinematics_Node_1Dof(Node):
         self.Glist = self.robot_model["Glist"]
         self.Blist = self.robot_model["Blist"]
 
+        # load simple robot URDF
         urdf_file_path_simple_robot = os.path.join(
             get_package_share_directory('simple_robot_1dof'), 'urdf', 'simple_robot.urdf')
         self.simple_robot_model = loadURDF(urdf_file_path_simple_robot)
@@ -43,15 +44,13 @@ class Forward_Kinematics_Node_1Dof(Node):
         self.simple_robot_Glist = self.simple_robot_model["Glist"]
         self.simple_robot_Blist = self.simple_robot_model["Blist"]
 
-
-        # create two publishers for the RViz simulation - one for the whole path and one for the marker
+        # publishers
         self.path_publisher_ = self.create_publisher(Path, '/simple_robot/visualization_path', 10)
         self.point_publisher_ = self.create_publisher(Marker, '/simple_robot/visualization_marker', 10)
-        # create a publisher for the plotjuggler angles
-        self.angle_publisher_ = self.create_publisher(Float64, '/simple_robot/angles_1dof/plotjuggler', 10)
-        # create a publisher for the pose
         self.pose_publisher_simple_robot_ = self.create_publisher(Pose, '/simple_robot/target_pose', 10)
         self.pose_publisher_kuka_robot_ = self.create_publisher(Pose, '/kuka_iiwa/target_pose', 10)
+        self.velocity_pub = self.create_publisher(Float64, '/velocity', 10)
+
         #RViz
         self.path = Path()
         self.path.header.frame_id = "map"
@@ -59,58 +58,58 @@ class Forward_Kinematics_Node_1Dof(Node):
 
         self.index = 0
 
+
     def listener_callback(self, msg):
         # Get the message
-        self.phi = msg.data
-        # call the transformation function
-        if self.phi == 0:
+        self.k = msg.data
+        # add 1 to index
+        if self.k == 0:
             self.index += 1
 
+        # call the transformation function
         self.transformation_simple_robot()
-
+    
 
     def transformation_simple_robot(self):
-        # Define the desired rotation
+        # define the desired rotation
         self.theta = - pi / 1.8
-        # Split theta
-        self.theta_fraction = self.phi * self.theta
-
-        self.publish_angle(self.theta_fraction)
+        # split theta for interpolation
+        self.theta_fraction = self.k * self.theta
 
-        # Define the matrix
         self.intermediate_rotation = np.array([
             [cos(self.theta_fraction), -sin(self.theta_fraction), 0, 0],
             [sin(self.theta_fraction), cos(self.theta_fraction), 0, 0],
             [0, 0, 1, 0],
             [0, 0, 0, 1]
         ])
-
+        # define a list of 0s
         self.simple_robot_joint_angles = np.zeros(len(self.simple_robot_Slist[0]))
-
+        # forward kinematics
         start_forward_kinematics_simple = FKinBody(self.simple_robot_M, self.simple_robot_Blist, self.simple_robot_joint_angles)
 
+        # apply the transformation
         end_forward_kinematics_simple = np.matmul(self.intermediate_rotation, start_forward_kinematics_simple)
-
+        # get the desired position and orientation
         end_position_simple = end_forward_kinematics_simple[:4, 3]
         end_orientation_simple = Rotation.from_matrix(end_forward_kinematics_simple[:3, :3]).as_quat()
-
-
+        # call the funct. publishing the desired pose
         self.publish_pose_simple_robot(end_position_simple, end_orientation_simple)
-        
+        # construct path
         self.construct_path(end_position_simple)
 
+        # move KUKA robot only once, when index equals 2 and let simulation with simple robot continue
         if self.index == 2:
             self.transformation_kuka()
 
 
     def transformation_kuka(self):
-
+        # list of joint states that may satisfy the desired pose
         kuka_joint_angles = np.zeros(len(self.Slist[0]))
         kuka_joint_angles[0] = np.pi
         kuka_joint_angles[1] = np.pi/2
         kuka_joint_angles[3] = np.pi/2
         kuka_joint_angles[5] = np.pi/2
-
+        # analog to simple robot
         start_forward_kinematics_kuka = FKinBody(self.M, self.Blist, kuka_joint_angles)
 
         end_forward_kinematics_kuka = np.matmul(self.intermediate_rotation, start_forward_kinematics_kuka)
@@ -118,7 +117,6 @@ class Forward_Kinematics_Node_1Dof(Node):
         end_position_kuka = end_forward_kinematics_kuka[:4, 3]
         end_orientation_kuka = Rotation.from_matrix(end_forward_kinematics_kuka[:3, :3]).as_quat()
 
-        
         self.publish_pose_kuka_pose(end_position_kuka, end_orientation_kuka)
 
         
@@ -134,15 +132,13 @@ class Forward_Kinematics_Node_1Dof(Node):
         pose.pose.orientation.w = 1.0
 
         # if the path is completed clear it
-        if self.phi == 0:
+        if self.k == 0:
             self.path.poses.clear()
         else:
             self.path.poses.append(pose)
 
-        # publish the path
         self.path_publisher_.publish(self.path)
 
-        # call the function publishing the marker
         self.publish_marker(pose.pose)
 
     def publish_marker(self, current_pose):
@@ -161,16 +157,9 @@ class Forward_Kinematics_Node_1Dof(Node):
         marker.color.g = 0.75
         marker.color.b = 0.0
 
-        # publish the marker
         self.point_publisher_.publish(marker)
 
 
-    def publish_angle(self, angle):
-        angle_msg = Float64()
-        angle_msg.data = angle * 180 / pi
-        self.angle_publisher_.publish(angle_msg)
-
-
     def publish_pose_simple_robot(self, vector, quaternion):
         pose_msg_sr = Pose()
         pose_msg_sr.position.x = vector[0]
@@ -183,7 +172,6 @@ class Forward_Kinematics_Node_1Dof(Node):
         self.pose_publisher_simple_robot_.publish(pose_msg_sr)
 
     def publish_pose_kuka_pose(self, vector, quaternion):    
-        # Create a Pose message for the target pose
         pose_msg = Pose()
         pose_msg.position.x = vector[0]
         pose_msg.position.y = vector[1]

ros2_ws/src/simple_robot_1dof/simple_robot_1dof/reference_node_1dof.py

@@ -10,9 +10,11 @@ class Reference_Node_1DoF(Node):
 
     def __init__(self):
         super().__init__('reference_node_1dof')
-        #Create a publisher sending the vector as a message
+        # publishers
         self.publisher_ = self.create_publisher(Float64, '/dof1/reference_angles', 10)
+        self.delta_publisher_ = self.create_publisher(Float64, '/delta', 10)
 
+        # parameters
         self.declare_parameter('max_velocity', 0.0)
         self.declare_parameter('acceleration', 0.0)
         self.declare_parameter('decelaration', 0.0)
@@ -25,12 +27,12 @@ class Reference_Node_1DoF(Node):
         self.velocity_constant = self.get_parameter('velocity_constant').get_parameter_value().double_value
         self.constant_speed = self.get_parameter('constant_speed').get_parameter_value().bool_value
 
-
-        self.phi_start = 0.0
-        self.phi_end = 1.0
-        self.phi = self.phi_start
+        self.index = 0
+        self.k_start = 0.0
+        self.k_end = 1.0
+        self.k = self.k_start
         self.current_velocity = 0.0
-
+        self.faktor = 1.3
         self.timer_period = 0.1  # seconds
         self.timer = self.create_timer(self.timer_period, self.ref_point_publisher)
         # List the points
@@ -41,26 +43,32 @@ class Reference_Node_1DoF(Node):
                
         msg = Float64()
 
+        # const. speed
         if self.constant_speed:
-            self.phi = self.phi + self.velocity_constant  
+            self.k = self.k + self.velocity_constant  
+        # trapez. velocity
         else:
-            if self.current_velocity < self.max_velocity and self.phi < 0.5:
-                self.phi = self.phi + self.current_velocity
-                self.current_velocity = self.current_velocity + self.timer_period * self.acceleration
-                self.transition_point = self.phi
-            elif self.phi > 1 - self.transition_point:
-                self.phi = self.phi + self.current_velocity
-                self.current_velocity = self.current_velocity - self.timer_period * self.decelaration
+            # acceleration phase
+            if self.current_velocity < self.max_velocity and self.k < 0.5:
+                self.k = self.k + self.current_velocity
+                self.current_velocity = self.current_velocity + self.acceleration
+                self.transition_point = self.k
+            # constant velocity
+            elif self.k >= self.k_end - self.faktor * self.transition_point:
+                self.k = self.k + self.current_velocity
+                self.current_velocity = self.current_velocity - self.decelaration
+            # decelaration phase
             else:
-                self.phi = self.phi + self.current_velocity
+                self.current_velocity = self.max_velocity
+                self.k = self.k + self.current_velocity
 
         #setting back to 0
-        if self.phi>self.phi_end:
-            self.phi = self.phi_start
+        if self.k>self.k_end:
+            self.k = self.k_start
             self.current_velocity = 0.0
-            # self.get_logger().info('Resetting "%f"' % self.phi)
+            self.index += 1
 
-        msg.data = self.phi
+        msg.data = self.k
         self.publisher_.publish(msg)