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

Clean code of 3dof simulation

parent a8831d91
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 72 additions and 213 deletions
ros2_ws/src/simple_robot_3dof/launch/simple_robot_rviz_simulation_3dof_launch.py
+ 3
− 3
View file @ 75734bf6
......@@ -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_3dof/simple_robot_3dof/forward_kinematics_node_3dof_1.py
+ 13
− 88
View file @ 75734bf6
......@@ -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]
......
ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_2.py
+ 17
− 85
View file @ 75734bf6
......@@ -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]
......
ros2_ws/src/simple_robot_3dof/simple_robot_3dof/reference_node_3dof.py
+ 35
− 33
View file @ 75734bf6
......@@ -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)
......
ros2_ws/src/simple_robot_3dof/urdf/simple_robot.urdf
+ 4
− 4
View file @ 75734bf6
......@@ -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>
......
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