From b9a251909312c00b03c313dce9ca3d7a80328e3f Mon Sep 17 00:00:00 2001
From: Ventsi <uquzq@student.kit.edu>
Date: Fri, 9 Aug 2024 00:27:54 +0200
Subject: [PATCH] Update forward_kinematics_node for the simple_robot
simulation
---
.../forward_kinematics_node_1dof.py | 53 ++++++++++++++++---
.../simple_robot_1dof/urdf/simple_robot.urdf | 8 +++
2 files changed, 55 insertions(+), 6 deletions(-)
diff --git a/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/forward_kinematics_node_1dof.py b/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/forward_kinematics_node_1dof.py
index 3ac27f8..440bb6f 100644
--- a/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/forward_kinematics_node_1dof.py
+++ b/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/forward_kinematics_node_1dof.py
@@ -35,6 +35,16 @@ class Forward_Kinematics_Node_1Dof(Node):
self.Glist = self.robot_model["Glist"]
self.Blist = self.robot_model["Blist"]
+ urdf_file_path_simple_robot = os.path.join(
+ get_package_share_directory('simple_robot_1dof'), '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"]
+
+
# 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)
@@ -57,9 +67,10 @@ class Forward_Kinematics_Node_1Dof(Node):
if self.phi == 0:
self.index += 1
- self.transformation()
+ self.transformation_kuka()
+ self.transformation_simple_robot()
- def transformation(self):
+ def transformation_kuka(self):
# Define the desired rotation here
theta = - pi / 1.8
# Split theta
@@ -92,13 +103,43 @@ class Forward_Kinematics_Node_1Dof(Node):
end_position = end_forward_kinematics[:4, 3]
end_orientation = Rotation.from_matrix(end_forward_kinematics[:3, :3]).as_quat()
-
- self.publish_pose_simple_robot(end_position, end_orientation)
-
if self.index == 2:
self.publish_pose_target_pose(end_position, end_orientation)
+
+
+ def transformation_simple_robot(self):
+ # Define the desired rotation here
+ theta = - pi / 1.8
+ # Split theta
+ theta_fraction = self.phi * theta
+
+ self.publish_angle(theta_fraction)
+
+ # 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]
+ ])
+
+ self.simple_robot_joint_angles = np.zeros(len(self.simple_robot_Slist[0]))
+ # self.simple_robot_joint_angles[1] = np.pi / 2
+
+ start_forward_kinematics_simple = FKinBody(self.simple_robot_M, self.simple_robot_Blist, self.simple_robot_joint_angles)
+
+ start_position_simple = start_forward_kinematics_simple[:4 ,3]
+ start_orientation = Rotation.from_matrix(start_forward_kinematics_simple[:3, :3]).as_quat()
+
+ end_forward_kinematics_simple = np.matmul(intermediate_rotation, start_forward_kinematics_simple)
+
+ 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.construct_path(end_position)
+ self.construct_path(end_position_simple)
def construct_path(self, vector):
# RViz
diff --git a/ros2_ws/src/simple_robot_1dof/urdf/simple_robot.urdf b/ros2_ws/src/simple_robot_1dof/urdf/simple_robot.urdf
index 2eb36ab..f9459f8 100644
--- a/ros2_ws/src/simple_robot_1dof/urdf/simple_robot.urdf
+++ b/ros2_ws/src/simple_robot_1dof/urdf/simple_robot.urdf
@@ -78,6 +78,14 @@
<limit effort = "300" lower="-2.96705972839" upper="2.96705972839" velocity = "10" />
</joint>
+ <link name="ee_link" />
+
+ <joint name="ee_joint" type="fixed">
+ <parent link="arm3"/>
+ <child link="ee_link"/>
+ <origin xyz="-0.3 0 0"/>
+ </joint>
+
</robot>
--
GitLab