Skip to content
Snippets Groups Projects
Commit a8831d91 authored by Ventsislav Dokusanski's avatar Ventsislav Dokusanski
Browse files

Clean code of 2dof simulations

parent 2fa567c5
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 55 additions and 130 deletions
ros2_ws/src/simple_robot_2dof/launch/simple_robot_rviz_simulation_2dof_launch.py
+ 3
− 3
View file @ a8831d91
......@@ -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(
......
ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py
+ 13
− 55
View file @ a8831d91
......@@ -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]
......
ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_2.py
+ 10
− 47
View file @ a8831d91
......@@ -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]
......
ros2_ws/src/simple_robot_2dof/simple_robot_2dof/reference_node_2dof.py
+ 25
− 21
View file @ a8831d91
......@@ -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)
......
ros2_ws/src/simple_robot_2dof/urdf/simple_robot.urdf
+ 4
− 4
View file @ a8831d91
......@@ -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>
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment

Help | Imprint | Privacy policy | Accessibility | Contact