diff --git a/ros2_ws/src/iiwa/iiwa_bringup/launch/__pycache__/iiwa.launch.cpython-310.pyc b/ros2_ws/src/iiwa/iiwa_bringup/launch/__pycache__/iiwa.launch.cpython-310.pyc
index d38891b2603662f601248eabcaa89ec7bc9d889e..dcd86a4512c5da509964ac1be6a3b8e2315f7db2 100644
Binary files a/ros2_ws/src/iiwa/iiwa_bringup/launch/__pycache__/iiwa.launch.cpython-310.pyc and b/ros2_ws/src/iiwa/iiwa_bringup/launch/__pycache__/iiwa.launch.cpython-310.pyc differ
diff --git a/ros2_ws/src/simple_robot_1dof/launch/kuka_rviz_simulation_1dof_launch.py b/ros2_ws/src/simple_robot_1dof/launch/kuka_rviz_simulation_1dof_launch.py
index c27d1ea7e46e9dcba98e86baa503e634d05b2f18..8b89fb919e669c60e8c24b56441b3d48c45e0b24 100644
--- a/ros2_ws/src/simple_robot_1dof/launch/kuka_rviz_simulation_1dof_launch.py
+++ b/ros2_ws/src/simple_robot_1dof/launch/kuka_rviz_simulation_1dof_launch.py
@@ -1,7 +1,7 @@
import os
from launch import LaunchDescription
from launch_ros.actions import Node
-from launch.actions import ExecuteProcess, DeclareLaunchArgument
+from launch.actions import DeclareLaunchArgument
from ament_index_python.packages import get_package_share_directory
from launch.substitutions import LaunchConfiguration
@@ -16,12 +16,6 @@ def generate_launch_description():
'forward_kinematics_simulations.rviz'
)
- # rviz_config_file_urdf = os.path.join(
- # get_package_share_directory(package_name),
- # 'rviz',
- # 'urdf.rviz'
- # )
-
urdf_dir = os.path.join(get_package_share_directory(package_name), 'urdf')
urdf_file = os.path.join(urdf_dir, 'simple_robot.urdf')
with open(urdf_file, 'r') as infp:
@@ -93,54 +87,34 @@ def generate_launch_description():
output='screen',
arguments=['-d', rviz_config_file]
)
- # rviz_node_urdf = Node(
- # package='rviz2',
- # executable='rviz2',
- # name='rviz2',
- # output='screen',
- # arguments=['-d', rviz_config_file_urdf]
- # )
- inverse_kinematics_node = Node(
+ kuka_inverse_kinematics_node = Node(
package= 'iiwa_inverse_kinematics',
executable='inverse_kinematics_node',
name='inverse_kinematics',
output='screen',
parameters = []
)
+ simple_robot_inverse_kinematics_node = Node(
+ package='simple_robot_inverse_kinematik',
+ executable='simple_robot_inverse_kinematik',
+ name='simple_robot_inverse_kinematik',
+ parameters=[]
+ )
robot_state_publisher = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='both',
parameters=[
{'robot_description': robot_desc}
- ]#,
- # remappings=[
- # ('/robot_description', 'simple_robot_description')
- # ]
- )
- joint_state_publisher = Node(
- package='joint_state_publisher',
- executable='joint_state_publisher',
- output = 'both',
- parameters=[
- {'robot_description': robot_desc}
- ]
- )
- joint_state_publisher_gui = Node(
- package='joint_state_publisher_gui',
- executable='joint_state_publisher_gui',
- name='joint_state_publisher_gui',
- output='screen',
+ ]
)
nodes = [
forward_kinematics_node,
reference_node,
rviz_node,
- # rviz_node_urdf,
- inverse_kinematics_node,
+ kuka_inverse_kinematics_node,
+ simple_robot_inverse_kinematics_node,
robot_state_publisher,
- joint_state_publisher,
- # joint_state_publisher_gui
]
return LaunchDescription(declared_arguments + nodes)
\ No newline at end of file
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 3331c8bfeb16f8a76e42b202201e5f28c98a9b77..c1346e42fc193b975d6447b84b6fc7ebf49a537e 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
@@ -11,7 +11,6 @@ from nav_msgs.msg import Path
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
@@ -67,70 +66,48 @@ class Forward_Kinematics_Node_1Dof(Node):
if self.phi == 0:
self.index += 1
- self.transformation_kuka()
self.transformation_simple_robot()
def transformation_kuka(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.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
+ 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 = FKinBody(self.M, self.Blist, self.joint_angles)
+ start_forward_kinematics_kuka = FKinBody(self.M, self.Blist, kuka_joint_angles)
- start_position = start_forward_kinematics[:4 ,3]
- start_orientation = Rotation.from_matrix(start_forward_kinematics[:3, :3]).as_quat()
-
- end_forward_kinematics = np.matmul(intermediate_rotation, start_forward_kinematics)
+ end_forward_kinematics = np.matmul(self.intermediate_rotation, start_forward_kinematics_kuka)
end_position = end_forward_kinematics[:4, 3]
end_orientation = Rotation.from_matrix(end_forward_kinematics[:3, :3]).as_quat()
- if self.index == 2:
- self.publish_pose_target_pose(end_position, end_orientation)
+
+ self.publish_pose_kuka_pose(end_position, end_orientation)
def transformation_simple_robot(self):
- # Define the desired rotation here
- theta = - pi / 1.8
+ # Define the desired rotation
+ self.theta = - pi / 1.8
# Split theta
- theta_fraction = self.phi * theta
+ self.theta_fraction = self.phi * self.theta
- self.publish_angle(theta_fraction)
+ self.publish_angle(self.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],
+ 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]
])
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_forward_kinematics_simple = np.matmul(self.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()
@@ -140,6 +117,9 @@ class Forward_Kinematics_Node_1Dof(Node):
self.construct_path(end_position_simple)
+ if self.index == 2:
+ self.transformation_kuka()
+
def construct_path(self, vector):
# RViz
pose = PoseStamped()
@@ -190,8 +170,6 @@ class Forward_Kinematics_Node_1Dof(Node):
def publish_pose_simple_robot(self, vector, quaternion):
pose_msg_sr = Pose()
- # pose_msg.header.frame_id = "map"
- # pose_msg.header.stamp = self.get_clock().now().to_msg()
pose_msg_sr.position.x = vector[0]
pose_msg_sr.position.y = vector[1]
pose_msg_sr.position.z = vector[2]
@@ -201,7 +179,7 @@ class Forward_Kinematics_Node_1Dof(Node):
pose_msg_sr.orientation.w = quaternion[3]
self.pose_publisher_simple_robot_.publish(pose_msg_sr)
- def publish_pose_target_pose(self, vector, quaternion):
+ 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]
@@ -212,8 +190,6 @@ class Forward_Kinematics_Node_1Dof(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(2)
def main(args = None):
diff --git a/ros2_ws/src/simple_robot_inverse_kinematik/setup.py b/ros2_ws/src/simple_robot_inverse_kinematik/setup.py
index 6112391abb8bdea92a88de327645862d493ed989..ae80ba467ae5171bac3b65910f880719621c3039 100644
--- a/ros2_ws/src/simple_robot_inverse_kinematik/setup.py
+++ b/ros2_ws/src/simple_robot_inverse_kinematik/setup.py
@@ -20,7 +20,7 @@ setup(
tests_require=['pytest'],
entry_points={
'console_scripts': [
- 'simple_robot_inverse = simple_robot_inverse_kinematik.inverse_kinematik:main',
+ 'simple_robot_inverse_kinematik = simple_robot_inverse_kinematik.inverse_kinematik:main',
],
},
)
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 4abe7a90aa7795ffae9f6fd5e2f4531e13aeeba2..2a491a6cd3982b1827fbac0cf9de472853a1631c 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
@@ -1,11 +1,9 @@
import os
-import re
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Pose
from sensor_msgs.msg import JointState
-from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from modern_robotics import IKinBody, RpToTrans, FKinBody
import numpy as np
from ament_index_python.packages import get_package_share_directory
@@ -44,9 +42,6 @@ class InverseKinematicsSimpleRobot(Node):
position = T[:3, 3]
self.get_logger().info('T: "%s":' %T)
- self.get_logger().info(f'Calculated Forward Kinematics Quaternion: {quat}')
- self.get_logger().info(f'Calculated Forward Kinematics Position: {position}')
-
def pose_callback(self, pose: Pose):
quat = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
r = Rotation.from_quat(quat)
@@ -56,26 +51,15 @@ class InverseKinematicsSimpleRobot(Node):
ev = 1e-4 # Fehlergrenze für Translation
joint_angles, success = IKinBody(self.Blist, self.M, T_sd, self.joint_angles, eomg, ev)
+ # self.get_logger().info('joint angles "%s"' % joint_angles)
if success:
- # self.joint_angles = joint_angles
-
-
joint_state_msg = JointState()
joint_state_msg.header.stamp = Clock().now().to_msg()
joint_state_msg.name = self.joint_names
joint_state_msg.position = joint_angles.tolist()
- # joint_trajectory_msg = JointTrajectory()
- # joint_trajectory_msg.joint_names = self.joint_names
- # point = JointTrajectoryPoint()
- # point.positions = joint_angles.tolist()
- # point.time_from_start.sec = 1 # Example time, adjust as needed
- # point.time_from_start.nanosec = 450000000 # Balint fragen, ob das verbessert werden kann
- # joint_trajectory_msg.points.append(point)
-
- # self.publisher.publish(joint_trajectory_msg)
self.publisher.publish(joint_state_msg)
else:
self.get_logger().error("Inverse Kinematics failed to find a solution.")