From e838ba3ecacca01db81b9d16adcdbbcfd3e8e476 Mon Sep 17 00:00:00 2001
From: Ventsi <uquzq@student.kit.edu>
Date: Mon, 12 Aug 2024 12:35:58 +0200
Subject: [PATCH] Fix urdf file, create urdf simulation for 2dof_2 and rename
launch files
---
...mple_robot_rviz_simulation_1dof_launch.py} | 0
ros2_ws/src/simple_robot_1dof/setup.py | 2 +-
...mple_robot_rviz_simulation_2dof_launch.py} | 0
ros2_ws/src/simple_robot_2dof/setup.py | 2 +-
.../forward_kinematics_node_2dof_1.py | 21 ++-
.../forward_kinematics_node_2dof_2.py | 169 ++++++++++++------
.../simple_robot_2dof/urdf/simple_robot.urdf | 4 +-
.../inverse_kinematik.py | 1 -
8 files changed, 133 insertions(+), 66 deletions(-)
rename ros2_ws/src/simple_robot_1dof/launch/{kuka_rviz_simulation_1dof_launch.py => simple_robot_rviz_simulation_1dof_launch.py} (100%)
rename ros2_ws/src/simple_robot_2dof/launch/{kuka_rviz_simulation_2dof_launch.py => simple_robot_rviz_simulation_2dof_launch.py} (100%)
diff --git a/ros2_ws/src/simple_robot_1dof/launch/kuka_rviz_simulation_1dof_launch.py b/ros2_ws/src/simple_robot_1dof/launch/simple_robot_rviz_simulation_1dof_launch.py
similarity index 100%
rename from ros2_ws/src/simple_robot_1dof/launch/kuka_rviz_simulation_1dof_launch.py
rename to ros2_ws/src/simple_robot_1dof/launch/simple_robot_rviz_simulation_1dof_launch.py
diff --git a/ros2_ws/src/simple_robot_1dof/setup.py b/ros2_ws/src/simple_robot_1dof/setup.py
index a4454a9..b8d38ed 100644
--- a/ros2_ws/src/simple_robot_1dof/setup.py
+++ b/ros2_ws/src/simple_robot_1dof/setup.py
@@ -12,7 +12,7 @@ setup(
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
- ('share/' + package_name + '/launch', ['launch/kuka_rviz_simulation_1dof_launch.py']),
+ ('share/' + package_name + '/launch', ['launch/simple_robot_rviz_simulation_1dof_launch.py']),
('share/' + package_name + '/rviz', ['rviz/forward_kinematics_simulations.rviz']),
(os.path.join('share', package_name, 'urdf'), glob('urdf/*.urdf')),
(os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
diff --git a/ros2_ws/src/simple_robot_2dof/launch/kuka_rviz_simulation_2dof_launch.py b/ros2_ws/src/simple_robot_2dof/launch/simple_robot_rviz_simulation_2dof_launch.py
similarity index 100%
rename from ros2_ws/src/simple_robot_2dof/launch/kuka_rviz_simulation_2dof_launch.py
rename to ros2_ws/src/simple_robot_2dof/launch/simple_robot_rviz_simulation_2dof_launch.py
diff --git a/ros2_ws/src/simple_robot_2dof/setup.py b/ros2_ws/src/simple_robot_2dof/setup.py
index 347d70b..bc62b9d 100644
--- a/ros2_ws/src/simple_robot_2dof/setup.py
+++ b/ros2_ws/src/simple_robot_2dof/setup.py
@@ -13,7 +13,7 @@ setup(
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
- ('share/' + package_name + '/launch', ['launch/kuka_rviz_simulation_2dof_launch.py']),
+ ('share/' + package_name + '/launch', ['launch/simple_robot_rviz_simulation_2dof_launch.py']),
('share/' + package_name + '/rviz', ['rviz/forward_kinematics_simulations.rviz']),
(os.path.join('share', package_name, 'urdf'), glob('urdf/*.urdf')),
(os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))),
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 78a2a3c..2d68a09 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
@@ -71,7 +71,7 @@ class Forward_Kinematics_Node_2Dof(Node):
# call the transformation function
self.transformation_simple_robot()
-
+
def transformation_kuka(self):
kuka_joint_angles = np.zeros(len(self.Slist[0]))
@@ -127,7 +127,24 @@ class Forward_Kinematics_Node_2Dof(Node):
self.construct_path(end_position_simple)
if self.index == 1:
- self.transformation_kuka()
+ self.transformation_kuka()
+
+ def transformation_kuka(self):
+
+ kuka_joint_angles = np.zeros(len(self.Slist[0]))
+ kuka_joint_angles[0] = np.pi
+ kuka_joint_angles[1] = np.pi/2
+ kuka_joint_angles[3] = np.pi/2
+ kuka_joint_angles[5] = np.pi/2
+
+ start_forward_kinematics_kuka = FKinBody(self.M, self.Blist, kuka_joint_angles)
+
+ end_forward_kinematics_kuka = np.matmul(self.intermediate_rotation, start_forward_kinematics_kuka)
+
+ end_position_kuka = end_forward_kinematics_kuka[:4, 3]
+ end_orientation_kuka = Rotation.from_matrix(end_forward_kinematics_kuka[:3, :3]).as_quat()
+
+ self.publish_pose_kuka_pose(end_position_kuka, end_orientation_kuka)
def construct_path(self, vector):
# RViz
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 047442a..625c645 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
@@ -8,12 +8,12 @@ from math import sin, cos, pi
import numpy as np
from std_msgs.msg import Float64
from nav_msgs.msg import Path
+from sensor_msgs.msg import JointState
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Pose
from visualization_msgs.msg import Marker
-import transformations
from scipy.spatial.transform import Rotation
-import time
+from rclpy.clock import Clock
class Forward_Kinematics_Node_2Dof(Node):
@@ -35,13 +35,33 @@ class Forward_Kinematics_Node_2Dof(Node):
self.Mlist = self.robot_model["Mlist"]
self.Glist = self.robot_model["Glist"]
self.Blist = self.robot_model["Blist"]
+ self.kuka_joint_angles = np.zeros(len(self.Slist[0]))
+ self.kuka_joint_angles[0] = np.pi
+ self.kuka_joint_angles[1] = np.pi/2
+ self.kuka_joint_angles[3] = np.pi/2
+ self.kuka_joint_angles[5] = np.pi/2
+
+ urdf_file_path_simple_robot = os.path.join(
+ get_package_share_directory('simple_robot_2dof'), 'urdf', 'simple_robot.urdf')
+ self.simple_robot_model = loadURDF(urdf_file_path_simple_robot)
+ self.simple_robot_M = self.simple_robot_model["M"]
+ self.simple_robot_Slist = self.simple_robot_model["Slist"]
+ self.simple_robot_Mlist = self.simple_robot_model["Mlist"]
+ self.simple_robot_Glist = self.simple_robot_model["Glist"]
+ self.simple_robot_Blist = self.simple_robot_model["Blist"]
+ 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
- self.pose_publisher_simple_robot_ = self.create_publisher(PoseStamped, '/simple_robot/end_effector_pose', 10)
+ self.joint_state_publisher_simple_robot_ = self.create_publisher(JointState, '/joint_states', 10)
+ 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()
@@ -59,84 +79,103 @@ class Forward_Kinematics_Node_2Dof(Node):
# check which function to call
if(self.index % 2 == 0):
# call the transformation function
- self.rotation()
+ self.rotation_simple_robot()
else:
- self.translation()
+ self.translation_simple_robot()
- def rotation(self):
+ def rotation_simple_robot(self):
# define the desired rotation
- theta = - pi / 2
+ self.theta = - pi / 2
# split theta
- theta_fraction = self.phi * theta
+ self.theta_fraction = self.phi * self.theta
+
+ self.h_fraction = 0.0
# define the matrix
- intermediate_rotation = np.array([
- [cos(theta_fraction), -sin(theta_fraction), 0, 0],
- [sin(theta_fraction), cos(theta_fraction), 0, 0],
+ 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],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
rotation = np.array([
- [cos(theta), -sin(theta), 0, 0],
- [sin(theta), cos(theta), 0, 0],
+ [cos(self.theta), -sin(self.theta), 0, 0],
+ [sin(self.theta), cos(self.theta), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]
])
- self.joint_angles = np.zeros(len(self.Slist[0]))
- self.joint_angles[0] = np.pi
- self.joint_angles[1] = np.pi/2
- self.joint_angles[3] = np.pi/2
- self.joint_angles[5] = np.pi/2
+ start_forward_kinematics_1_sr = FKinBody(self.simple_robot_M, self.simple_robot_Blist, self.simple_robot_joint_angles)
- start_forward_kinematics = FKinBody(self.M, self.Blist, self.joint_angles)
+ start_position_1_sr = start_forward_kinematics_1_sr[:4, 3]
- start_position = start_forward_kinematics[:4, 3]
- start_orientation = Rotation.from_matrix(start_forward_kinematics[:3, :3]).as_quat()
+ self.end_forward_kinematics_1_sr = np.matmul(self.intermediate_rotation, start_forward_kinematics_1_sr)
- self.end_forward_kinematics_1 = np.matmul(intermediate_rotation, start_forward_kinematics)
+ end_position_1_sr = self.end_forward_kinematics_1_sr[:4, 3]
- end_position_1 = self.end_forward_kinematics_1[:4, 3]
- end_orientation_1 = Rotation.from_matrix(self.end_forward_kinematics_1[:3, :3]).as_quat()
+ # 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 = np.matmul(rotation, start_position)
+ self.end_vector_sr = np.matmul(rotation, start_position_1_sr)
- self.publish_pose_simple_robot(end_position_1, end_orientation_1)
+ self.publish_joint_states_simple_robot()
if self.index == 2:
- self.publish_pose_target_pose(end_position_1, end_orientation_1)
+ self.rotation_kuka()
# construct the path of the rotation
- self.construct_path(end_position_1)
+ self.construct_path(end_position_1_sr)
- def translation(self):
+ def translation_simple_robot(self):
# define the desired height
- h = 0.3
+ self.h = 0.3
# split h
- h_fraction = self.phi * h
+ self.h_fraction = self.phi * self.h
# define translation matrix
- intermediate_translation = np.array([
+ self.intermediate_translation = np.array([
[1, 0, 0, 0],
[0, 1, 0, 0],
- [0, 0, 1, h_fraction],
+ [0, 0, 1, self.h_fraction],
[0, 0, 0, 1]
])
- end_forward_kinematics_2 = np.matmul(intermediate_translation, self.end_forward_kinematics_1)
+ end_forward_kinematics_2_sr= np.matmul(self.intermediate_translation, self.end_forward_kinematics_1_sr)
- end_position_2 = end_forward_kinematics_2[:4, 3]
- end_orientation_2 = Rotation.from_matrix(end_forward_kinematics_2[:3, :3]).as_quat()
+ 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_pose_simple_robot(end_position_2, end_orientation_2)
+
+ self.publish_joint_states_simple_robot()
if self.index == 3:
- self.publish_pose_target_pose(end_position_2, end_orientation_2)
+ self.translation_kuka()
# construct the path of the translation
- self.construct_path(end_position_2)
+ self.construct_path(end_position_2_sr)
+
+ def rotation_kuka(self):
+
+ start_forward_kinematics_kuka = FKinBody(self.M, self.Blist, self.kuka_joint_angles)
+
+ self.end_forward_kinematics_kuka_1 = np.matmul(self.intermediate_rotation, start_forward_kinematics_kuka)
+
+ 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):
+
+ end_forward_kinematics_kuka_2 = np.matmul(self.intermediate_translation, self.end_forward_kinematics_kuka_1)
+
+ end_position_kuka_2 = end_forward_kinematics_kuka_2[:4, 3]
+ end_orientation_kuka_2 = Rotation.from_matrix(end_forward_kinematics_kuka_2[:3, :3]).as_quat()
+
+ self.publish_pose_kuka_pose(end_position_kuka_2, end_orientation_kuka_2)
+
def construct_path(self, vector):
# RViz
@@ -168,9 +207,9 @@ class Forward_Kinematics_Node_2Dof(Node):
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.g = 0.0
@@ -180,20 +219,35 @@ class Forward_Kinematics_Node_2Dof(Node):
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_pose_target_pose(self, vector, quaternion):
+ # 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_states[1] = self.h_fraction
+ joint_states[0] = self.theta_fraction
+
+ joint_state_msg = JointState()
+ joint_state_msg.header.stamp = Clock().now().to_msg()
+
+ joint_state_msg.name = joint_names
+ joint_state_msg.position = joint_states
+
+ 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]
@@ -204,9 +258,6 @@ class Forward_Kinematics_Node_2Dof(Node):
pose_msg.orientation.z = quaternion[2]
pose_msg.orientation.w = quaternion[3]
self.pose_publisher_kuka_robot_.publish(pose_msg)
- # if self.phi == 0.0:
- # time.sleep(1)
-
def main(args = None):
rclpy.init(args = args)
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 f662cae..712896f 100644
--- a/ros2_ws/src/simple_robot_2dof/urdf/simple_robot.urdf
+++ b/ros2_ws/src/simple_robot_2dof/urdf/simple_robot.urdf
@@ -23,9 +23,9 @@
<link name="arm1">
<visual>
- <origin xyz="0 0 0.35" rpy="0 0 0"/>
+ <origin xyz="0 0 0.405" rpy="0 0 0"/>
<geometry>
- <cylinder length="0.6" radius="0.019"/>
+ <cylinder length="0.71" radius="0.019"/>
</geometry>
<material name="red">
<color rgba="0.75 0.0 0.0 1"/>
diff --git a/ros2_ws/src/simple_robot_inverse_kinematik/simple_robot_inverse_kinematik/inverse_kinematik.py b/ros2_ws/src/simple_robot_inverse_kinematik/simple_robot_inverse_kinematik/inverse_kinematik.py
index 47061e8..f459080 100644
--- a/ros2_ws/src/simple_robot_inverse_kinematik/simple_robot_inverse_kinematik/inverse_kinematik.py
+++ b/ros2_ws/src/simple_robot_inverse_kinematik/simple_robot_inverse_kinematik/inverse_kinematik.py
@@ -58,7 +58,6 @@ class InverseKinematicsSimpleRobot(Node):
joint_state_msg.header.stamp = Clock().now().to_msg()
joint_state_msg.name = self.joint_names
joint_state_msg.position = joint_angles.tolist()
- self.get_logger().info('joint_angles "%s":' %joint_state_msg.position)
self.publisher.publish(joint_state_msg)
else:
--
GitLab