diff --git a/ros2_ws/src/simple_robot_2dof/launch/simple_robot_rviz_simulation_2dof_launch.py b/ros2_ws/src/simple_robot_2dof/launch/simple_robot_rviz_simulation_2dof_launch.py
index 145b493fe00c9c9932a7cd157c5891aa8f4bd44b..3b122d42dd4f559e57053abdab97cfc8368843c3 100644
--- a/ros2_ws/src/simple_robot_2dof/launch/simple_robot_rviz_simulation_2dof_launch.py
+++ b/ros2_ws/src/simple_robot_2dof/launch/simple_robot_rviz_simulation_2dof_launch.py
@@ -25,19 +25,19 @@ 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(
diff --git a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py
index 7cdbb44857bc6861d42ac8db580a3a855b63b778..8938b0e6f23ee9bdc592e2533f798b1b795f342b 100644
--- a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py
+++ b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py
@@ -20,7 +20,7 @@ class Forward_Kinematics_Node_2Dof(Node):
def __init__(self):
super().__init__('forward_kinematics_node_2dof')
- # create a subscriber
+ # subscriber
self.subscription = self.create_subscription(
Float64,
'/dof2/reference_angles',
@@ -47,16 +47,11 @@ class Forward_Kinematics_Node_2Dof(Node):
self.simple_robot_joint_angles = np.zeros(len(self.simple_robot_Slist[0]))
- # Create two publishers for the RViz simulation - one for the whole path and one for the marker
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 data
- self.angle_publisher_ = self.create_publisher(Float64, '/simple_robot/angle_2dof/plotjuggler', 10)
- self.height_publisher_ = self.create_publisher(Float64, '/simple_robot/height_2dof/plotjuggler', 10)
- # create a publisher for the pose
self.joint_state_publisher_simple_robot_ = self.create_publisher(JointState, '/joint_states', 10)
- # 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)
+
#RViz
self.path = Path()
self.path.header.frame_id = "map"
@@ -66,9 +61,9 @@ class Forward_Kinematics_Node_2Dof(Node):
def listener_callback(self, msg):
# Get the message
- self.phi = msg.data
+ self.k = msg.data
- if self.phi == 0:
+ if self.k == 0:
self.index += 1
# call the transformation function
@@ -76,21 +71,14 @@ class Forward_Kinematics_Node_2Dof(Node):
def transformation_simple_robot(self):
- # Define the desired rotation
- self.theta = - pi / 1.8
- # Split theta
- self.theta_fraction = self.phi * self.theta
-
- self.publish_angle(self.theta_fraction)
-
- # Definte the desired height
- self.h = 0.2
- #Split h
- self.h_fraction = self.phi * self.h
+ # define the desired rotation
+ self.theta = - pi / 2
+ self.theta_fraction = self.k * self.theta
- self.publish_height(self.h_fraction)
+ # definte the desired height
+ self.h = 0.3
+ self.h_fraction = self.k * self.h
- # 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],
@@ -104,8 +92,6 @@ class Forward_Kinematics_Node_2Dof(Node):
end_position_simple = end_forward_kinematics_simple[:4, 3]
- # end_orientation_simple = Rotation.from_matrix(end_forward_kinematics_simple[:3, :3]).as_quat()
- # self.publish_pose_simple_robot(end_position_simple, end_orientation_simple)
self.publish_joint_states_simple_robot()
self.construct_path(end_position_simple)
@@ -140,20 +126,16 @@ class Forward_Kinematics_Node_2Dof(Node):
pose.pose.position.z = vector[2]
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):
- # RViz
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = self.get_clock().now().to_msg()
@@ -164,35 +146,12 @@ class Forward_Kinematics_Node_2Dof(Node):
marker.scale.y = 0.025
marker.scale.z = 0.025
marker.color.a = 1.0
- marker.color.r = 1.0
+ marker.color.r = 0.0
marker.color.g = 0.0
- marker.color.b = 0.0
+ marker.color.b = 0.75
- # 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)
- # self.get_logger().info('Publishing: %f' %angle_radians)
-
- def publish_height(self, height):
- height_msg = Float64()
- height_msg.data = height
- self.height_publisher_.publish(height_msg)
-
- # def publish_pose_simple_robot(self, vector, quaternion):
- # pose_msg_sr = Pose()
- # pose_msg_sr.position.x = vector[0]
- # pose_msg_sr.position.y = vector[1]
- # pose_msg_sr.position.z = vector[2]
- # pose_msg_sr.orientation.x = quaternion[0]
- # pose_msg_sr.orientation.y = quaternion[1]
- # pose_msg_sr.orientation.z = quaternion[2]
- # pose_msg_sr.orientation.w = quaternion[3]
- # self.pose_publisher_simple_robot_.publish(pose_msg_sr)
-
def publish_joint_states_simple_robot(self):
joint_names = ['joint1', 'joint2']
@@ -211,7 +170,6 @@ class Forward_Kinematics_Node_2Dof(Node):
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]
diff --git a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_2.py b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_2.py
index 625c645fb273854d586b0457a3fd3a6fd1e74452..bdf15fb9da754e9f09fe8be862366104d8ea05d0 100644
--- a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_2.py
+++ b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_2.py
@@ -20,7 +20,6 @@ class Forward_Kinematics_Node_2Dof(Node):
def __init__(self):
super().__init__('forward_kinematics_node_2dof')
- # Create a subscriber, initialising the transformation
self.subscription = self.create_subscription(
Float64,
'/dof2/reference_angles',
@@ -52,7 +51,6 @@ class Forward_Kinematics_Node_2Dof(Node):
self.simple_robot_joint_angles = np.zeros(len(self.simple_robot_Slist[0]))
- # Create two publishers for the RViz simulation - one for the whole path and one for the marker
self.path_publisher_ = self.create_publisher(Path, '/simple_robot/visualization_path', 10)
self.point_publisher_ = self.create_publisher(Marker, '/simple_robot/visualization_marker', 10)
self.index = 0
@@ -61,8 +59,8 @@ class Forward_Kinematics_Node_2Dof(Node):
self.h_fraction = 0.0
self.theta_fraction = 0.0
- # self.pose_publisher_simple_robot_ = self.create_publisher(PoseStamped, '/simple_robot/end_effector_pose', 10)
self.pose_publisher_kuka_robot_ = self.create_publisher(Pose, '/kuka_iiwa/target_pose', 10)
+
#RViz
self.path = Path()
self.path.header.frame_id = "map"
@@ -70,15 +68,12 @@ class Forward_Kinematics_Node_2Dof(Node):
def listener_callback(self, msg):
- # Get the message
- self.phi = msg.data
- # Define index and if phi is resetted add 1 to the index
- if(self.phi == 0):
+ self.k = msg.data
+ if self.k == 0.0:
self.index += 1
# check which function to call
if(self.index % 2 == 0):
- # call the transformation function
self.rotation_simple_robot()
else:
self.translation_simple_robot()
@@ -87,12 +82,11 @@ class Forward_Kinematics_Node_2Dof(Node):
def rotation_simple_robot(self):
# define the desired rotation
self.theta = - pi / 2
- # split theta
- self.theta_fraction = self.phi * self.theta
+ # split theta for interpolation
+ self.theta_fraction = self.k * self.theta
self.h_fraction = 0.0
- # 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],
@@ -114,10 +108,6 @@ class Forward_Kinematics_Node_2Dof(Node):
end_position_1_sr = self.end_forward_kinematics_1_sr[:4, 3]
- # end_orientation_1 = Rotation.from_matrix(self.end_forward_kinematics_1_simple[:3, :3]).as_quat()
- # self.publish_pose_simple_robot(end_position_1, end_orientation_1)
-
- # calculate the end vector
self.end_vector_sr = np.matmul(rotation, start_position_1_sr)
self.publish_joint_states_simple_robot()
@@ -125,16 +115,13 @@ class Forward_Kinematics_Node_2Dof(Node):
if self.index == 2:
self.rotation_kuka()
- # construct the path of the rotation
self.construct_path(end_position_1_sr)
def translation_simple_robot(self):
# define the desired height
self.h = 0.3
- # split h
- self.h_fraction = self.phi * self.h
+ self.h_fraction = self.k * self.h
- # define translation matrix
self.intermediate_translation = np.array([
[1, 0, 0, 0],
[0, 1, 0, 0],
@@ -145,16 +132,12 @@ class Forward_Kinematics_Node_2Dof(Node):
end_forward_kinematics_2_sr= np.matmul(self.intermediate_translation, self.end_forward_kinematics_1_sr)
end_position_2_sr = end_forward_kinematics_2_sr[:4, 3]
- # end_orientation_2 = Rotation.from_matrix(end_forward_kinematics_2_sr[:3, :3]).as_quat()
-
- # self.publish_pose_simple_robot(end_position_2, end_orientation_2)
self.publish_joint_states_simple_robot()
if self.index == 3:
self.translation_kuka()
- # construct the path of the translation
self.construct_path(end_position_2_sr)
def rotation_kuka(self):
@@ -165,6 +148,7 @@ class Forward_Kinematics_Node_2Dof(Node):
end_position_kuka_1 = self.end_forward_kinematics_kuka_1[:4, 3]
end_orientation_kuka_1 = Rotation.from_matrix(self.end_forward_kinematics_kuka_1[:3, :3]).as_quat()
+
self.publish_pose_kuka_pose(end_position_kuka_1, end_orientation_kuka_1)
def translation_kuka(self):
@@ -178,7 +162,6 @@ class Forward_Kinematics_Node_2Dof(Node):
def construct_path(self, vector):
- # RViz
pose = PoseStamped()
pose.header.frame_id = "map"
pose.header.stamp = self.get_clock().now().to_msg()
@@ -187,20 +170,16 @@ class Forward_Kinematics_Node_2Dof(Node):
pose.pose.position.z = vector[2]
pose.pose.orientation.w = 1.0
- #If the path is completed clear it
- if (self.phi == 0 and self.index % 2 == 0):
+ if (self.k == 0 and self.index % 2 == 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):
- # RViz
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = self.get_clock().now().to_msg()
@@ -215,27 +194,12 @@ class Forward_Kinematics_Node_2Dof(Node):
marker.color.g = 0.0
marker.color.b = 0.0
- # Publish the marker
self.point_publisher_.publish(marker)
-
- # def publish_pose_simple_robot(self, vector, quaternion):
- # pose_msg = PoseStamped()
- # pose_msg.header.frame_id = "map"
- # pose_msg.header.stamp = self.get_clock().now().to_msg()
- # pose_msg.pose.position.x = vector[0]
- # pose_msg.pose.position.y = vector[1]
- # pose_msg.pose.position.z = vector[2]
- # pose_msg.pose.orientation.x = quaternion[0]
- # pose_msg.pose.orientation.y = quaternion[1]
- # pose_msg.pose.orientation.z = quaternion[2]
- # pose_msg.pose.orientation.w = quaternion[3]
- # self.pose_publisher_simple_robot_.publish(pose_msg)
-
def publish_joint_states_simple_robot(self):
- joint_names = ['joint1', 'joint2', 'joint3', 'joint4']
- joint_states = [0.0, 0.0, 0.0, 0.0]
+ joint_names = ['joint1', 'joint2']
+ joint_states = [0.0, 0.0]
joint_states[1] = self.h_fraction
joint_states[0] = self.theta_fraction
@@ -248,7 +212,6 @@ class Forward_Kinematics_Node_2Dof(Node):
self.joint_state_publisher_simple_robot_.publish(joint_state_msg)
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]
diff --git a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/reference_node_2dof.py b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/reference_node_2dof.py
index 843662b0e69692565416a2c03c955a27fce6be97..cc47f4c1ec9948c698c629774a909d3fa5817d62 100644
--- a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/reference_node_2dof.py
+++ b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/reference_node_2dof.py
@@ -10,7 +10,6 @@ class Reference_Node_2DoF(Node):
def __init__(self):
super().__init__('reference_node_2dof')
- #Create a publisher sending the vector as a message
self.publisher_ = self.create_publisher(Float64, '/dof2/reference_angles', 10)
self.declare_parameter('max_velocity', 0.0)
@@ -25,43 +24,48 @@ class Reference_Node_2DoF(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.index = 0
- self.phi_start = 0.0
- self.phi_end = 1.0
- self.phi = self.phi_start
+ self.k_start = 0.0
+ self.k_end = 1.0
+ self.k = self.k_start
self.transition_point = 0.0
- self.timer_period = 0.1 # seconds
+ self.timer_period = 0.1
+ self.faktor = 1.285
self.timer = self.create_timer(self.timer_period, self.ref_point_publisher)
self.current_velocity = self.timer_period * self.acceleration
- # List the points
-
+
+
def ref_point_publisher(self):
# Ref position over the time
# Interpolate between x_0 - x_1
msg = Float64()
+ # const. velocity
if self.constant_speed == True:
- self.phi = self.phi + self.velocity_constant
+ self.k = self.k + self.velocity_constant
+ # trapez. velocity
elif self.constant_speed == False:
- 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
+ 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
+ elif self.k > 1 - self.faktor * self.transition_point:
+ self.k = self.k + self.current_velocity
+ self.current_velocity = self.current_velocity - self.decelaration
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 = self.timer_period * self.acceleration
- # self.get_logger().info('Resetting "%f"' % self.phi)
+ self.index += 1
+
+ msg.data = self.k
- msg.data = self.phi
self.publisher_.publish(msg)
diff --git a/ros2_ws/src/simple_robot_2dof/urdf/simple_robot.urdf b/ros2_ws/src/simple_robot_2dof/urdf/simple_robot.urdf
index eafb87db9143fef78f3a9b41c62e3a57c8b7a88a..a3a6f45a334efec293cb541262091f53e1234325 100644
--- a/ros2_ws/src/simple_robot_2dof/urdf/simple_robot.urdf
+++ b/ros2_ws/src/simple_robot_2dof/urdf/simple_robot.urdf
@@ -86,8 +86,8 @@
<geometry>
<cylinder length="0.3" radius="0.02"/>
</geometry>
- <material name="green">
- <color rgba="0.0 0.75 0.0 1"/>
+ <material name="blue">
+ <color rgba="0.0 0.0 0.75 1"/>
</material>
</visual>
<inertial>
@@ -109,8 +109,8 @@
<geometry>
<cylinder length="0.3" radius="0.02"/>
</geometry>
- <material name="green">
- <color rgba="0.0 0.75 0.0 1"/>
+ <material name="blue">
+ <color rgba="0.0 0.0 0.75 1.0"/>
</material>
</visual>
<inertial>