diff --git a/ros2_ws/src/simple_robot_3dof/launch/simple_robot_rviz_simulation_3dof_launch.py b/ros2_ws/src/simple_robot_3dof/launch/simple_robot_rviz_simulation_3dof_launch.py
index 90ccb37e67fa828499000fd6c4f933ee78ebbe30..7ccd1b0b0f0269393ce40055250ad348195f9e80 100644
--- a/ros2_ws/src/simple_robot_3dof/launch/simple_robot_rviz_simulation_3dof_launch.py
+++ b/ros2_ws/src/simple_robot_3dof/launch/simple_robot_rviz_simulation_3dof_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_3dof/simple_robot_3dof/forward_kinematics_node_3dof_1.py b/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_1.py
index ea7f6fd14320adfe618388bd902b37b233b6ac77..82c3b7c22b43f28c40a4022d04a540fbdaddb3d3 100644
--- a/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_1.py
+++ b/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_1.py
@@ -20,7 +20,6 @@ class Forward_Kinematics_Node_3Dof(Node):
def __init__(self):
super().__init__('forward_kinematics_node_3dof')
- # Create a subscriber, initialising the transformation
self.subscription = self.create_subscription(
Float64,
'/dof3/reference_angles',
@@ -51,15 +50,9 @@ class Forward_Kinematics_Node_3Dof(Node):
self.simple_robot_Blist = self.simple_robot_model["Blist"]
self.simple_robot_joint_angles = [0.0, 0.0, 0.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.joint_state_publisher_simple_robot_ = self.create_publisher(JointState, '/joint_states', 10)
-
- # 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"
@@ -68,53 +61,39 @@ class Forward_Kinematics_Node_3Dof(Node):
self.index = 0
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
+
self.transformation_simple_robot()
def transformation_simple_robot(self):
- # Define the desired rotation
+ # 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
- # Definte the desired height
- self.h = 0.2
- #Split h
- self.h_fraction = self.phi * self.h
+ # definte the desired height
+ self.h = 0.15
+ self.h_fraction = self.k * self.h
# define the desired length
- self.d = 0.1
- # split d
- self.d_fraction = self.phi * self.d
+ self.d = 0.12
+ self.d_fraction = self.k * self.d
- # Define the matrix
self.intermediate_translation = np.array([
[cos(self.theta_fraction), -sin(self.theta_fraction), 0, 0],
[sin(self.theta_fraction), cos(self.theta_fraction), 0, self.d_fraction],
[0, 0, 1, self.h_fraction],
[0, 0, 0, 1]
])
-
- start_forward_kinematics_sr = FKinBody(self.simple_robot_M, self.simple_robot_Blist, self.simple_robot_joint_angles)
-
- end_forward_kinematics_sr = np.matmul(self.intermediate_translation, start_forward_kinematics_sr)
-
-
- end_position_sr = end_forward_kinematics_sr[:4, 3]
-
self.publish_joint_states_simple_robot()
- # end_orientation_sr = Rotation.from_matrix(end_forward_kinematics_sr[:3, :3]).as_quat()
- # self.publish_pose_simple_robot(end_position_sr, end_orientation_sr)
+
if self.index == 1:
self.transformation_kuka()
- self.construct_path(end_position_sr)
def transformation_kuka(self):
@@ -126,47 +105,6 @@ class Forward_Kinematics_Node_3Dof(Node):
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 construct_path(self, vector):
- # RViz
- pose = PoseStamped()
- pose.header.frame_id = "map"
- pose.header.stamp = self.get_clock().now().to_msg()
- pose.pose.position.x = vector[0]
- pose.pose.position.y = vector[1]
- pose.pose.position.z = vector[2]
- pose.pose.orientation.w = 1.0
-
- #If the path is completed clear it
- if self.phi == 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()
- marker.type = Marker.SPHERE
- marker.action = Marker.ADD
- marker.pose = current_pose
- marker.scale.x = 0.05
- marker.scale.y = 0.05
- marker.scale.z = 0.05
- marker.color.a = 1.0
- marker.color.r = 1.0
- marker.color.g = 0.0
- marker.color.b = 0.0
-
- # Publish the marker
- self.point_publisher_.publish(marker)
-
def publish_joint_states_simple_robot(self):
joint_names = ['joint1', 'joint2', 'joint3']
@@ -183,21 +121,8 @@ class Forward_Kinematics_Node_3Dof(Node):
self.joint_state_publisher_simple_robot_.publish(joint_state_msg)
- # 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_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_3dof/simple_robot_3dof/forward_kinematics_node_3dof_2.py b/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_2.py
index ffa832c5d78309b34dea201cee771a8377cc7613..5a03852e13131df465cc5da0684fa03a40b2e71f 100644
--- a/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_2.py
+++ b/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_2.py
@@ -20,7 +20,6 @@ class Forward_Kinematics_Node_3Dof(Node):
def __init__(self):
super().__init__('forward_kinematics_node_3dof')
- # Create a subscriber, initialising the transformation
self.subscription = self.create_subscription(
Float64,
'/dof3/reference_angles',
@@ -51,20 +50,15 @@ class Forward_Kinematics_Node_3Dof(Node):
self.simple_robot_Blist = self.simple_robot_model["Blist"]
self.simple_robot_joint_angles = [0.0, 0.0, 0.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
- self.angle_publisher_ = self.create_publisher(Float64, '/simple_robot/angle_3dof/plotjuggler',10)
- self.height_publisher_ = self.create_publisher(Float64, '/simple_robot/height_3dof/plotjuggler', 10)
- self.length_publisher_ = self.create_publisher(Float64, '/simple_robot/length_3dof/plotjuggler', 10)
-
self.joint_state_publisher_simple_robot_ = self.create_publisher(JointState, '/joint_states', 10)
+ self.pose_publisher_kuka_robot_ = self.create_publisher(Pose, '/kuka_iiwa/target_pose', 10)
self.theta_fraction = 0.0
self.h_fraction = 0.0
self.d_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)
+ self.index = 0
+
#RViz
self.path = Path()
self.path.header.frame_id = "map"
@@ -72,15 +66,11 @@ class Forward_Kinematics_Node_3Dof(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.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
+ if self.index % 2 == 0:
self.translation_simple_robot()
else:
self.rotation_simple_robot()
@@ -89,21 +79,14 @@ class Forward_Kinematics_Node_3Dof(Node):
def translation_simple_robot(self):
# definte the desired height
self.h = 0.25
- # split h
- self.h_fraction = self.phi * self.h
-
- self.publish_height(self.h_fraction)
+ self.h_fraction = self.k * self.h
# define the desired length
self.d = 0.2
- # split d
- self.d_fraction = self.phi * self.d
+ self.d_fraction = self.k * self.d
self.theta_fraction = 0.0
- self.publish_length(self.d_fraction)
-
- # define translation matrix
self.intermediate_translation = np.array([
[1, 0, 0, 0],
[0, 1, 0, self.d_fraction],
@@ -111,39 +94,27 @@ class Forward_Kinematics_Node_3Dof(Node):
[0, 0, 0, 1]
])
- # translation_matrix = np.array([
- # [1, 0, 0, 0],
- # [0, 1, 0, self.d],
- # [0, 0, 1, self.h],
- # [0, 0, 0, 1]
- # ])
-
start_forward_kinematics_1_sr = FKinBody(self.simple_robot_M, self.simple_robot_Blist, self.simple_robot_joint_angles)
self.end_forward_kinematics_1_sr = np.matmul(self.intermediate_translation, start_forward_kinematics_1_sr)
self.end_position_1_sr = self.end_forward_kinematics_1_sr[:4, 3]
- # end_orientation_1_sr = Rotation.from_matrix(self.end_forward_kinematics_1[:3, :3]).as_quat()
- # self.publish_pose_simple_robot(self.end_position_1_sr, end_orientation_1_sr)
+
self.publish_joint_states_simple_robot()
if self.index == 2:
self.kuka_translation()
- # self.publish_pose_target_pose(end_position_1, end_orientation_1)
- # construct the path of the translation
self.construct_path(self.end_position_1_sr)
def rotation_simple_robot(self):
# define the desired rotation
self.theta = - pi / 2
- # split theta
- self.theta_fraction = self.phi * self.theta
+ self.theta_fraction = self.k * self.theta
- self.publish_angle(self.theta_fraction)
+ self.h2_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],
@@ -155,15 +126,11 @@ class Forward_Kinematics_Node_3Dof(Node):
end_position_2_sr = end_forward_kinematics_2[:4, 3]
- # end_orientation_2_sr = Rotation.from_matrix(end_forward_kinematics_2[: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.kuka_rotation()
- # self.publish_pose_target_pose(end_position_2_sr, end_orientation_2_sr)
- # construct the path of the rotation
self.construct_path(end_position_2_sr)
def kuka_translation(self):
@@ -186,7 +153,6 @@ class Forward_Kinematics_Node_3Dof(Node):
def construct_path(self, vector):
- # RViz
pose = PoseStamped()
pose.header.frame_id = "map"
pose.header.stamp = self.get_clock().now().to_msg()
@@ -195,65 +161,32 @@ class Forward_Kinematics_Node_3Dof(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()
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.pose = current_pose
- marker.scale.x = 0.05
- marker.scale.y = 0.05
- marker.scale.z = 0.05
+ marker.scale.x = 0.025
+ 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)
-
- def publish_height(self, height):
- height_msg = Float64()
- height_msg.data = height
- self.height_publisher_.publish(height_msg)
-
- def publish_length(self, length):
- length_msg = Float64()
- length_msg.data = length
- self.length_publisher_.publish(length_msg)
-
- # 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']
@@ -271,7 +204,6 @@ class Forward_Kinematics_Node_3Dof(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_3dof/simple_robot_3dof/reference_node_3dof.py b/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/reference_node_3dof.py
index 26f50dfd0f731f0fd8383222d5e5e14aea0029bb..7a78684f499d1337c57f0b4ca2f537a646aa9e96 100644
--- a/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/reference_node_3dof.py
+++ b/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/reference_node_3dof.py
@@ -10,9 +10,9 @@ class Reference_Node_3DoF(Node):
def __init__(self):
super().__init__('reference_node_1dof')
- #Create a publisher sending the vector as a message
self.publisher_ = self.create_publisher(Float64, '/dof3/reference_angles', 10)
+ # parameters
self.declare_parameter('max_velocity', 0.0)
self.declare_parameter('acceleration', 0.0)
self.declare_parameter('decelaration', 0.0)
@@ -27,19 +27,19 @@ class Reference_Node_3DoF(Node):
self.constant_speed = self.get_parameter('constant_speed').get_parameter_value().bool_value
self.simultaneous = self.get_parameter('simultaneous').get_parameter_value().bool_value
-
- 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.faktor = 1.28
self.index = 0
- self.timer_period = 0.1 # seconds
+ self.timer_period = 0.1
self.timer = self.create_timer(self.timer_period, self.ref_point_publisher)
- self.current_velocity = self.timer_period * self.acceleration
- # List the points
+ self.current_velocity = self.acceleration
def ref_point_publisher(self):
# Ref position over the time
@@ -47,44 +47,46 @@ class Reference_Node_3DoF(Node):
msg = Float64()
+ # check which movement is initialised
if self.simultaneous:
+ # 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.k = self.k + self.current_velocity
else:
if self.constant_speed == True:
- self.phi = self.phi + self.current_velocity
+ self.k = self.k + self.velocity_constant
elif self.constant_speed == False:
if self.index % 2 == 0:
- self.phi = self.phi + self.velocity_constant
+ self.k = self.k + self.velocity_constant
else:
- if self.current_velocity < self.max_velocity:
- 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.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.index += 1
- self.current_velocity = self.timer_period * self.acceleration
- # self.get_logger().info('Resetting "%f"' % self.phi)
+ self.current_velocity = self.acceleration
+
+ msg.data = self.k
- msg.data = self.phi
self.publisher_.publish(msg)
diff --git a/ros2_ws/src/simple_robot_3dof/urdf/simple_robot.urdf b/ros2_ws/src/simple_robot_3dof/urdf/simple_robot.urdf
index 61f8b81418e0d822141b76d1414c9c479b8e3517..a51ea7a385e2e128e3932ead59f03326de902797 100644
--- a/ros2_ws/src/simple_robot_3dof/urdf/simple_robot.urdf
+++ b/ros2_ws/src/simple_robot_3dof/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>
@@ -111,8 +111,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>