Skip to content
Snippets Groups Projects
Commit a27057db authored by uquzq's avatar uquzq
Browse files

Update the simulations of the 3 dof movements in RViz

parent 98ca4b38
Branches
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_1.py
+ 69
− 83
View file @ a27057db
import rclpy
from rclpy.node import Node
from math import sin, cos
from math import sin, cos, pi
import numpy as np
import sympy as sp
from std_msgs.msg import Float32MultiArray
from std_msgs.msg import Float64
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
from visualization_msgs.msg import Marker
......@@ -11,93 +10,83 @@ from visualization_msgs.msg import Marker
class Forward_Kinematics_Node_3Dof(Node):
def __init__(self):
super().__init__('forward_kinematics_node_3dof')
#Create a subscriber, initialising the transformation
super().__init__('forward_kinematics_node_1dof')
# Create a subscriber, initialising the transformation
self.subscription = self.create_subscription(
Float32MultiArray,
'topic',
self.transformation,
Float64,
'/dof2/reference_angles',
self.listener_callback,
10)
#Create two publishers for the RViz simulation - one for the whole path and one for the marker
self.publisher_ = self.create_publisher(Path, 'visualization_path', 10)
# Create two publishers for the RViz simulation - one for the whole path and one for the marker
self.path_publisher_ = self.create_publisher(Path, 'visualization_path', 10)
self.point_publisher_ = self.create_publisher(Marker, 'visualization_marker', 10)
def transformation(self, msg):
#Get the message
vector = np.array(msg.data)
#Define the desired rotation
theta = - sp.pi / 2
#Define the desired height
h = 1.0
#Define the desired length
d = 1.0
#Define the period of the timer
timer_period = 0.03
#Create the timer calling the function placing the marker along the path
self.timer = self.create_timer(timer_period, self.marker_function)
self.path = None
self.current_index = 0
#RViz
self.path = Path()
self.path.header.frame_id = "map"
self.path.header.stamp = self.get_clock().now().to_msg()
#Calculate more points in order to visualise the path of the robot(for example 150)
iterations = 150
for i in range(iterations):
fraction = i / (iterations - 1)
#Calculate the intermediate rotation matrix
intermediate_rotation_matrix = np.array([
[cos(fraction * theta), -sin(fraction * theta), 0, 0],
[sin(fraction * theta), cos(fraction * theta), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
#Calculate the intermediate translation matrix
intermediate_translation_matrix = np.array([
[1, 0, 0, d * fraction],
[0, 1, 0, 0],
[0, 0, 1, h * fraction],
[0, 0, 0, 1]
])
#Calculate the intermediate combined matrix
intermediate_combined_matrix = np.matmul(intermediate_translation_matrix, intermediate_rotation_matrix)
#Calculate the intermediate vector
intermediate_vector = np.matmul(intermediate_combined_matrix, vector)
#RViz
pose = PoseStamped()
pose.header.frame_id = "map"
pose.header.stamp = self.get_clock().now().to_msg()
pose.pose.position.x = intermediate_vector[0]
pose.pose.position.y = intermediate_vector[1]
pose.pose.position.z = intermediate_vector[2]
pose.pose.orientation.w = 1.0
def listener_callback(self, msg):
# Get the message
self.phi = msg.data
# call the transformation function
self.transformation()
def transformation(self):
# Define the desired rotation
theta = -pi / 2
# Split theta
theta_fraction = self.phi * theta
# Definte the desired height
h = 1
#Split h
h_fraction = self.phi * h
# define the desired length
d = 0.6
# split d
d_fraction = self.phi * d
# Define the vector
vector = np.array([-1.0, 0.0, 0.0, 1.0])
# Define the matrix
intermediate_translation = np.array([
[cos(theta_fraction), -sin(theta_fraction), 0, d_fraction],
[sin(theta_fraction), cos(theta_fraction), 0, 0],
[0, 0, 1, h_fraction],
[0, 0, 0, 1]
])
# Calculate the intermediate vectors
intermediate_vector = np.matmul(intermediate_translation, vector)
self.construct_path(intermediate_vector)
def construct_path(self, vector_path):
# RViz
pose = PoseStamped()
pose.header.frame_id = "map"
pose.header.stamp = self.get_clock().now().to_msg()
pose.pose.position.x = vector_path[0]
pose.pose.position.y = vector_path[1]
pose.pose.position.z = vector_path[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.publisher_.publish(self.path)
# Publish the path
self.path_publisher_.publish(self.path)
def marker_function(self):
#If path is not initialised, issue a mistake
if self.path is None:
self.get_logger().info('Path is None')
return
#Set the index to 0, if the whole movement has already been already completed
if self.current_index >= len(self.path.poses):
self.current_index = 0
# Call the function publishing the marker
self.publish_marker(pose.pose)
#Define a variable that defines the current position of the marker
current_pose = self.path.poses[self.current_index].pose
#RViz
def publish_marker(self, current_pose):
# RViz
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = self.get_clock().now().to_msg()
......@@ -111,12 +100,9 @@ class Forward_Kinematics_Node_3Dof(Node):
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
self.point_publisher_.publish(marker)
self.current_index +=1
# Publish the marker
self.point_publisher_.publish(marker)
def main(args = None):
rclpy.init(args = args)
......
ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_2.py
+ 104
− 94
View file @ a27057db
import rclpy
from rclpy.node import Node
from math import sin, cos
from math import sin, cos, pi
import numpy as np
import sympy as sp
from std_msgs.msg import Float32MultiArray
from std_msgs.msg import Float64
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
from visualization_msgs.msg import Marker
class Forward_Kinematics_Node_3Dof(Node):
def __init__(self):
super().__init__('forward_kinematics_node_3dof')
#Create a subscriber, initialising the transformation
super().__init__('forward_kinematics_node_1dof')
# Create a subscriber, initialising the transformation
self.subscription = self.create_subscription(
Float32MultiArray,
'topic',
self.transformation,
Float64,
'/dof2/reference_angles',
self.listener_callback,
10)
#Create two publishers for the RViz simulation - one for the whole path and one for the marker
self.publisher_ = self.create_publisher(Path, 'visualization_path', 10)
# Create two publishers for the RViz simulation - one for the whole path and one for the marker
self.path_publisher_ = self.create_publisher(Path, 'visualization_path', 10)
self.point_publisher_ = self.create_publisher(Marker, 'visualization_marker', 10)
def transformation(self, msg):
#Get the message
vector = np.array(msg.data)
#Define the desired rotation
theta = - sp.pi / 2
#Define the desired height
h = 0.5
#Define the desired length
d = 0.5
#Define the period of the timer
timer_period = 0.03
#Create the timer calling the function placing the marker along the path
self.timer = self.create_timer(timer_period, self.marker_function)
self.path = None
self.current_index = 0
self.index = 0
#RViz
self.path = Path()
self.path.header.frame_id = "map"
self.path.header.stamp = self.get_clock().now().to_msg()
#Calculate more points in order to visualise the path of the robot(for example 150)
iterations = 150
for i in range(iterations):
fraction_1 = i / (iterations - 1)
#Calculate the intermediate translation matrix
intermediate_translation_matrix = np.array([
[1, 0, 0, 0],
[0, 1, 0, d * fraction_1],
[0, 0, 1, h * fraction_1],
[0, 0, 0, 1]
])
#Calculate the first intermediate vector
intermediate_vector_1 = np.matmul(intermediate_translation_matrix, vector)
#RViz
pose = PoseStamped()
pose.header.frame_id = "map"
pose.header.stamp = self.get_clock().now().to_msg()
pose.pose.position.x = intermediate_vector_1[0]
pose.pose.position.y = intermediate_vector_1[1]
pose.pose.position.z = intermediate_vector_1[2]
pose.pose.orientation.w = 1.0
self.path.poses.append(pose)
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.index +=1
# check which function to call
if(self.index % 2 == 0):
# call the transformation function
self.translation()
else:
self.rotation()
for j in range(iterations):
fraction_2 = j / (iterations -1)
#Calculate the intermediate rotation matrix
intermediate_rotation_matrix = np.array([
[cos(fraction_2 * theta), -sin(fraction_2 * theta), 0, 0],
[sin(fraction_2 * theta), cos(fraction_2 * theta), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
#Calculate the second intermediate vector
intermediate_vector_2 = np.matmul(intermediate_rotation_matrix, intermediate_vector_1)
#RViz
pose = PoseStamped()
pose.header.frame_id = "map"
pose.header.stamp = self.get_clock().now().to_msg()
pose.pose.position.x = intermediate_vector_2[0]
pose.pose.position.y = intermediate_vector_2[1]
pose.pose.position.z = intermediate_vector_2[2]
pose.pose.orientation.w = 1.0
def rotation(self):
# define the desired rotation
theta = -pi / 2
# split theta
theta_fraction = self.phi * theta
# define the matrix
intermediate_rotation = np.array([
[cos(theta_fraction), -sin(theta_fraction), 0, 0],
[sin(theta_fraction), cos(theta_fraction), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
# calculate the intermediate vectors
rotated_intermediate_vector = np.matmul(intermediate_rotation, self.end_vector)
# construct the path of the rotation
self.construct_path(rotated_intermediate_vector)
def translation(self):
# definte the desired height
h = 0.6
# split h
h_fraction = self.phi * h
# define the desired length
d = 0.35
# split d
d_fraction = self.phi * d
# Define the vector
vector = np.array([-1.0, 0.0, 0.0, 1.0])
# define translation matrix
intermediate_translation = np.array([
[1, 0, 0, 0],
[0, 1, 0, d_fraction],
[0, 0, 1, h_fraction],
[0, 0, 0, 1]
])
translation_matrix = np.array([
[1, 0, 0, 0],
[0, 1, 0, d],
[0, 0, 1, h],
[0, 0, 0, 1]
])
# calculate the intermediate vectors
translated_vector = np.matmul(intermediate_translation, vector)
# calculate the end vector
self.end_vector = np.matmul(translation_matrix, vector)
# construct the path of the translation
self.construct_path(translated_vector)
def construct_path(self, vector_path):
# RViz
pose = PoseStamped()
pose.header.frame_id = "map"
pose.header.stamp = self.get_clock().now().to_msg()
pose.pose.position.x = vector_path[0]
pose.pose.position.y = vector_path[1]
pose.pose.position.z = vector_path[2]
pose.pose.orientation.w = 1.0
#If the path is completed clear it
if self.phi == 0 and self.index % 2 == 0:
self.path.poses.clear()
else:
self.path.poses.append(pose)
#Publish the path
self.publisher_.publish(self.path)
# Publish the path
self.path_publisher_.publish(self.path)
# Call the function publishing the marker
self.publish_marker(pose.pose)
def marker_function(self):
#If path is not initialised, issue a mistake
if self.path is None:
self.get_logger().info('Path is None')
return
#Set the index to 0, if the whole movement has already been already completed
if self.current_index >= len(self.path.poses):
self.current_index = 0
#Define a variable that defines the current position of the marker
current_pose = self.path.poses[self.current_index].pose
#RViz
def publish_marker(self, current_pose):
# RViz
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = self.get_clock().now().to_msg()
......@@ -126,11 +138,9 @@ class Forward_Kinematics_Node_3Dof(Node):
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
self.point_publisher_.publish(marker)
self.current_index +=1
# Publish the marker
self.point_publisher_.publish(marker)
def main(args = None):
......
ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_3.py
+ 102
− 94
View file @ a27057db
import rclpy
from rclpy.node import Node
from math import sin, cos
from math import sin, cos, pi
import numpy as np
import sympy as sp
from std_msgs.msg import Float32MultiArray
from std_msgs.msg import Float64
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
from visualization_msgs.msg import Marker
class Forward_Kinematics_Node_3Dof(Node):
def __init__(self):
super().__init__('forward_kinematics_node_3dof')
#Create a subscriber, initialising the transformation
super().__init__('forward_kinematics_node_1dof')
# Create a subscriber, initialising the transformation
self.subscription = self.create_subscription(
Float32MultiArray,
'topic',
self.transformation,
Float64,
'/dof2/reference_angles',
self.listener_callback,
10)
#Create two publishers for the RViz simulation - one for the whole path and one for the marker
self.publisher_ = self.create_publisher(Path, 'visualization_path', 10)
# Create two publishers for the RViz simulation - one for the whole path and one for the marker
self.path_publisher_ = self.create_publisher(Path, 'visualization_path', 10)
self.point_publisher_ = self.create_publisher(Marker, 'visualization_marker', 10)
def transformation(self, msg):
#Get the message
vector = np.array(msg.data)
#Define the desired rotation
theta = - sp.pi / 2
#Define the desired height
h = 0.5
#Define the desired length
d = 0.5
#Define the period of the timer
timer_period = 0.03
#Create the timer calling the function placing the marker along the path
self.timer = self.create_timer(timer_period, self.marker_function)
self.path = None
self.current_index = 0
self.index = 0
#RViz
self.path = Path()
self.path.header.frame_id = "map"
self.path.header.stamp = self.get_clock().now().to_msg()
#Calculate more points in order to visualise the path of the robot(for example 150)
iterations = 150
for i in range(iterations):
fraction_1 = i / (iterations - 1)
#Calculate the intermediate combined matrix
intermediate_combined_matrix = np.array([
[cos(fraction_1 * theta), -sin(fraction_1 * theta), 0, 0],
[sin(fraction_1 * theta), cos(fraction_1 * theta), 0, 0],
[0, 0, 1, h * fraction_1],
[0, 0, 0, 1]
])
#Calculate the first intermediate vector
intermediate_vector_1 = np.matmul(intermediate_combined_matrix, vector)
#RViz
pose = PoseStamped()
pose.header.frame_id = "map"
pose.header.stamp = self.get_clock().now().to_msg()
pose.pose.position.x = intermediate_vector_1[0]
pose.pose.position.y = intermediate_vector_1[1]
pose.pose.position.z = intermediate_vector_1[2]
pose.pose.orientation.w = 1.0
self.path.poses.append(pose)
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.index +=1
# check which function to call
if(self.index % 2 == 0):
# call the transformation function
self.translation()
else:
self.rotation()
for j in range(iterations):
fraction_2 = j / (iterations -1)
#Calculate the intermediate y - rotation matrix
intermediate_rotation_matrix = np.array([
[1, 0, 0, 0],
[0, cos(fraction_2 * theta), -sin(fraction_2 * theta), 0],
[0, sin(fraction_2 * theta), cos(fraction_2 * theta), 0],
[0, 0, 0, 1]
])
#Calculate the second intermediate vector
intermediate_vector_2 = np.matmul(intermediate_rotation_matrix, intermediate_vector_1)
#RViz
pose = PoseStamped()
pose.header.frame_id = "map"
pose.header.stamp = self.get_clock().now().to_msg()
pose.pose.position.x = intermediate_vector_2[0]
pose.pose.position.y = intermediate_vector_2[1]
pose.pose.position.z = intermediate_vector_2[2]
pose.pose.orientation.w = 1.0
def translation(self):
# define the desired rotation
theta = -pi / 2
# split theta
theta_fraction = self.phi * theta
# definte the desired height
h = 0.5
# split h
h_fraction = self.phi * h
# Define the vector
vector = np.array([-1.0, 0.0, 0.0, 1.0])
# define the matrix
intermediate_translation = np.array([
[cos(theta_fraction), -sin(theta_fraction), 0, 0],
[sin(theta_fraction), cos(theta_fraction), 0, 0],
[0, 0, 1, h_fraction],
[0, 0, 0, 1]
])
translation_matrix = np.array([
[cos(theta), -sin(theta), 0, 0],
[sin(theta), cos(theta), 0, 0],
[0, 0, 1, h],
[0, 0, 0, 1]
])
# calculate the intermediate vectors
rotated_intermediate_vector = np.matmul(intermediate_translation, vector)
self.end_vector = np.matmul(translation_matrix, vector)
# construct the path of the rotation
self.construct_path(rotated_intermediate_vector)
def rotation(self):
# define the desired rotation
alpha = - pi / 6
# split alpha
alpha_fraction = self.phi * alpha
# define the matrix
intermediate_rotation = np.array([
[1, 0, 0, 0],
[0, cos(alpha_fraction), -sin(alpha_fraction), 0],
[0, sin(alpha_fraction), cos(alpha_fraction), 0],
[0, 0, 0, 1]
])
# calculate the intermediate vectors
rotated_vector = np.matmul(intermediate_rotation, self.end_vector)
# construct the path of the translation
self.construct_path(rotated_vector)
def construct_path(self, vector_path):
# RViz
pose = PoseStamped()
pose.header.frame_id = "map"
pose.header.stamp = self.get_clock().now().to_msg()
pose.pose.position.x = vector_path[0]
pose.pose.position.y = vector_path[1]
pose.pose.position.z = vector_path[2]
pose.pose.orientation.w = 1.0
#If the path is completed clear it
if self.phi == 0 and self.index %2 == 0:
self.path.poses.clear()
else:
self.path.poses.append(pose)
#Publish the path
self.publisher_.publish(self.path)
def marker_function(self):
#If path is not initialised, issue a mistake
if self.path is None:
self.get_logger().info('Path is None')
return
#Set the index to 0, if the whole movement has already been already completed
if self.current_index >= len(self.path.poses):
self.current_index = 0
# Publish the path
self.path_publisher_.publish(self.path)
#Define a variable that defines the current position of the marker
current_pose = self.path.poses[self.current_index].pose
# Call the function publishing the marker
self.publish_marker(pose.pose)
#RViz
def publish_marker(self, current_pose):
# RViz
marker = Marker()
marker.header.frame_id = "map"
marker.header.stamp = self.get_clock().now().to_msg()
......@@ -127,11 +137,9 @@ class Forward_Kinematics_Node_3Dof(Node):
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
self.point_publisher_.publish(marker)
self.current_index +=1
# Publish the marker
self.point_publisher_.publish(marker)
def main(args = None):
......
ros2_ws/src/simple_robot_3dof/simple_robot_3dof/reference_node_3dof.py
+ 26
− 13
View file @ a27057db
import rclpy
from rclpy.node import Node
import numpy as np
from std_msgs.msg import Float32MultiArray
from std_msgs.msg import Float64
from geometry_msgs.msg import Point, PoseStamped
import time
class Reference_Node_3DoF(Node):
def __init__(self):
super().__init__('reference_node_3dof')
self.publisher_ = self.create_publisher(Float32MultiArray, 'topic', 10)
#Wait 4 seconds
time.sleep(4)
#Call the function publishing the vector
self.publish_info()
super().__init__('reference_node_1dof')
#Create a publisher sending the vector as a message
self.publisher_ = self.create_publisher(Float64, '/dof2/reference_angles', 10)#
timer_period = 0.05 # seconds
self.timer = self.create_timer(timer_period, self.ref_point_publisher)
self.phi_start = 0.0
self.phi_end = 1.0
self.phi = self.phi_start
self.delta = 0.005
# List the points
def publish_info(self):
phi_vector = [-1.0, 0.0, 0.0, 1.0]
msg = Float32MultiArray()
msg.data = phi_vector
self.publisher_.publish(msg)
self.get_logger().info('Publishing:"%s"' % msg.data)
def ref_point_publisher(self):
# Ref position over the time
# Interpolate between x_0 - x_1
msg = Float64()
self.phi = self.phi + self.delta
#setting back to 0
if self.phi>self.phi_end:
self.phi = self.phi_start
#self.get_logger().info('Resetting "%f"' % self.phi)
msg.data = self.phi
self.publisher_.publish(msg)
def main(args = None):
rclpy.init(args = args)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment

Help | Imprint | Privacy policy | Accessibility | Contact