diff --git a/ros2_ws/src/simple_robot_1dof/rviz/forward_kinematics_simulations.rviz b/ros2_ws/src/simple_robot_1dof/rviz/forward_kinematics_simulations.rviz
index 632e356b8513781bbc387dfdb67c1e346bfcb811..e6d557b9c250f68199af9b8ff83e0ce80a99277a 100644
--- a/ros2_ws/src/simple_robot_1dof/rviz/forward_kinematics_simulations.rviz
+++ b/ros2_ws/src/simple_robot_1dof/rviz/forward_kinematics_simulations.rviz
@@ -133,6 +133,7 @@ Visualization Manager:
Alpha: 1
Show Axes: false
Show Trail: false
+ Value: true
Mass Properties:
Inertia: false
Mass: false
@@ -157,7 +158,7 @@ Visualization Manager:
Value: false
Enabled: true
Global Options:
- Background Color: 48; 48; 48
+ Background Color: 61; 56; 70
Fixed Frame: map
Frame Rate: 30
Name: root
@@ -216,10 +217,10 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
- Pitch: 0.9647961854934692
+ Pitch: 1.149795651435852
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
- Yaw: 4.703580856323242
+ Yaw: 4.598585605621338
Saved: ~
Window Geometry:
Displays:
@@ -237,5 +238,5 @@ Window Geometry:
Views:
collapsed: false
Width: 1908
- X: 130
+ X: 140
Y: 164
diff --git a/ros2_ws/src/simple_robot_1dof/rviz/urdf.rviz b/ros2_ws/src/simple_robot_1dof/rviz/urdf.rviz
deleted file mode 100644
index 424e079802ec0c80a81328803903c91d56b31cf0..0000000000000000000000000000000000000000
--- a/ros2_ws/src/simple_robot_1dof/rviz/urdf.rviz
+++ /dev/null
@@ -1,206 +0,0 @@
-Panels:
- - Class: rviz_common/Displays
- Help Height: 138
- Name: Displays
- Property Tree Widget:
- Expanded:
- - /Global Options1
- - /Status1
- - /RobotModel1
- Splitter Ratio: 0.5
- Tree Height: 340
- - Class: rviz_common/Selection
- Name: Selection
- - Class: rviz_common/Tool Properties
- Expanded:
- - /2D Goal Pose1
- - /Publish Point1
- Name: Tool Properties
- Splitter Ratio: 0.5886790156364441
- - Class: rviz_common/Views
- Expanded:
- - /Current View1
- Name: Views
- Splitter Ratio: 0.5
- - Class: rviz_common/Time
- Experimental: false
- Name: Time
- SyncMode: 0
- SyncSource: ""
-Visualization Manager:
- Class: ""
- Displays:
- - Alpha: 0.5
- Cell Size: 1
- Class: rviz_default_plugins/Grid
- Color: 160; 160; 164
- Enabled: true
- Line Style:
- Line Width: 0.029999999329447746
- Value: Lines
- Name: Grid
- Normal Cell Count: 0
- Offset:
- X: 0
- Y: 0
- Z: 0
- Plane: XY
- Plane Cell Count: 10
- Reference Frame: <Fixed Frame>
- Value: true
- - Alpha: 1
- Class: rviz_default_plugins/RobotModel
- Collision Enabled: false
- Description File: ""
- Description Source: Topic
- Description Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /robot_description
- Enabled: true
- Links:
- All Links Enabled: true
- Expand Joint Details: false
- Expand Link Details: false
- Expand Tree: false
- Link Tree Style: Links in Alphabetic Order
- arm1:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- arm2:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- arm3:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- base_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- Mass Properties:
- Inertia: false
- Mass: false
- Name: RobotModel
- TF Prefix: ""
- Update Interval: 0
- Value: true
- Visual Enabled: true
- - Class: rviz_default_plugins/TF
- Enabled: true
- Frame Timeout: 15
- Frames:
- All Enabled: true
- arm1:
- Value: true
- arm2:
- Value: true
- arm3:
- Value: true
- base_link:
- Value: true
- Marker Scale: 1
- Name: TF
- Show Arrows: true
- Show Axes: true
- Show Names: false
- Tree:
- base_link:
- arm1:
- arm2:
- arm3:
- {}
- Update Interval: 0
- Value: true
- Enabled: true
- Global Options:
- Background Color: 48; 48; 48
- Fixed Frame: base_link
- Frame Rate: 30
- Name: root
- Tools:
- - Class: rviz_default_plugins/Interact
- Hide Inactive Objects: true
- - Class: rviz_default_plugins/MoveCamera
- - Class: rviz_default_plugins/Select
- - Class: rviz_default_plugins/FocusCamera
- - Class: rviz_default_plugins/Measure
- Line color: 128; 128; 0
- - Class: rviz_default_plugins/SetInitialPose
- Covariance x: 0.25
- Covariance y: 0.25
- Covariance yaw: 0.06853891909122467
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /initialpose
- - Class: rviz_default_plugins/SetGoal
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /goal_pose
- - Class: rviz_default_plugins/PublishPoint
- Single click: true
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /clicked_point
- Transformation:
- Current:
- Class: rviz_default_plugins/TF
- Value: true
- Views:
- Current:
- Class: rviz_default_plugins/Orbit
- Distance: 1.8436399698257446
- Enable Stereo Rendering:
- Stereo Eye Separation: 0.05999999865889549
- Stereo Focal Distance: 1
- Swap Stereo Eyes: false
- Value: false
- Focal Point:
- X: 0
- Y: 0
- Z: 0
- Focal Shape Fixed Size: true
- Focal Shape Size: 0.05000000074505806
- Invert Z Axis: false
- Name: Current View
- Near Clip Distance: 0.009999999776482582
- Pitch: 0.6997970938682556
- Target Frame: <Fixed Frame>
- Value: Orbit (rviz)
- Yaw: 1.5303975343704224
- Saved: ~
-Window Geometry:
- Displays:
- collapsed: false
- Height: 846
- Hide Left Dock: false
- Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd0000000400000000000001f70000024cfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e0000024c0000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f0000024cfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e0000024c0000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005810000005afc0100000002fb0000000800540069006d00650100000000000005810000058100fffffffb0000000800540069006d00650100000000000004500000000000000000000002130000024c00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
- Selection:
- collapsed: false
- Time:
- collapsed: false
- Tool Properties:
- collapsed: false
- Views:
- collapsed: false
- Width: 1409
- X: 965
- Y: 227
diff --git a/ros2_ws/src/simple_robot_1dof/setup.py b/ros2_ws/src/simple_robot_1dof/setup.py
index 860d6d7d9faa340a524d6026f1f75709b17dd3e4..a4454a9436e9168eb72f05b235ea3064cc99942c 100644
--- a/ros2_ws/src/simple_robot_1dof/setup.py
+++ b/ros2_ws/src/simple_robot_1dof/setup.py
@@ -16,7 +16,6 @@ setup(
('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]*'))),
- # (os.path.join('share', package_name), glob('urdf/*')),
],
install_requires=['setuptools'],
zip_safe=True,
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 c1346e42fc193b975d6447b84b6fc7ebf49a537e..8bb289ec7c12df87514606e101221bb60c9692d2 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
@@ -12,7 +12,7 @@ from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Pose
from visualization_msgs.msg import Marker
from scipy.spatial.transform import Rotation
-import time
+
class Forward_Kinematics_Node_1Dof(Node):
@@ -78,13 +78,13 @@ class Forward_Kinematics_Node_1Dof(Node):
start_forward_kinematics_kuka = FKinBody(self.M, self.Blist, kuka_joint_angles)
- end_forward_kinematics = np.matmul(self.intermediate_rotation, start_forward_kinematics_kuka)
+ end_forward_kinematics_kuka = 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()
+ 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, end_orientation)
+ self.publish_pose_kuka_pose(end_position_kuka, end_orientation_kuka)
def transformation_simple_robot(self):
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 f9459f8dd8e1c6fa74607c4311569ee71fa014d6..22afcb72da5b9bbb8b848df53399c761387bba40 100644
--- a/ros2_ws/src/simple_robot_1dof/urdf/simple_robot.urdf
+++ b/ros2_ws/src/simple_robot_1dof/urdf/simple_robot.urdf
@@ -1,7 +1,17 @@
<?xml version="1.0"?>
<robot name="simple_robot">
-
- <link name="map" />
+
+ <link name="map">
+ <visual>
+ <origin xyz="0 0 0.05" rpy="0 0 0"/>
+ <geometry>
+ <box size = "0.5 0.5 0.1"/>
+ </geometry>
+ <material name="grey">
+ <color rgba="0.5 0.5 0.5 1"/>
+ </material>
+ </visual>
+ </link>
<link name="arm1">
<visual>
@@ -10,7 +20,7 @@
<cylinder length="0.71" radius="0.02"/>
</geometry>
<material name="red">
- <color rgba="1.0 0.0 0.0 1"/>
+ <color rgba="0.75 0.0 0.0 1"/>
</material>
</visual>
<inertial>
@@ -35,7 +45,7 @@
<cylinder length="0.3" radius="0.02"/>
</geometry>
<material name="green">
- <color rgba="0.0 1.0 0.0 1"/>
+ <color rgba="0.0 0.75 0.0 1"/>
</material>
</visual>
<inertial>
@@ -60,7 +70,7 @@
<cylinder length="0.3" radius="0.02"/>
</geometry>
<material name="blue">
- <color rgba="0.0 0.0 1.0 1"/>
+ <color rgba="0.0 0.0 0.75 1"/>
</material>
</visual>
<inertial>
diff --git a/ros2_ws/src/simple_robot_2dof/launch/kuka_rviz_simulation_2dof_launch.py b/ros2_ws/src/simple_robot_2dof/launch/kuka_rviz_simulation_2dof_launch.py
index a88263688befc5d33805652c8c4b3cee3bc01bf1..43b9271ba94adaed8433c04c0c7fc6542c5ebd8a 100644
--- a/ros2_ws/src/simple_robot_2dof/launch/kuka_rviz_simulation_2dof_launch.py
+++ b/ros2_ws/src/simple_robot_2dof/launch/kuka_rviz_simulation_2dof_launch.py
@@ -16,6 +16,11 @@ def generate_launch_description():
'forward_kinematics_simulations.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:
+ robot_desc = infp.read()
+
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
@@ -105,13 +110,29 @@ def generate_launch_description():
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}
+ ]
+ )
nodes = [
forward_kinematics_node_1,
forward_kinematics_node_2,
reference_node,
rviz_node,
- inverse_kinematic_node
+ inverse_kinematic_node,
+ simple_robot_inverse_kinematics_node,
+ robot_state_publisher
]
return LaunchDescription(declared_arguments + nodes)
\ No newline at end of file
diff --git a/ros2_ws/src/simple_robot_2dof/rviz/forward_kinematics_simulations.rviz b/ros2_ws/src/simple_robot_2dof/rviz/forward_kinematics_simulations.rviz
index cbad83eb69d23555060457acea15f1b896b79e01..e6d557b9c250f68199af9b8ff83e0ce80a99277a 100644
--- a/ros2_ws/src/simple_robot_2dof/rviz/forward_kinematics_simulations.rviz
+++ b/ros2_ws/src/simple_robot_2dof/rviz/forward_kinematics_simulations.rviz
@@ -1,6 +1,6 @@
Panels:
- Class: rviz_common/Displays
- Help Height: 78
+ Help Height: 138
Name: Displays
Property Tree Widget:
Expanded:
@@ -9,8 +9,9 @@ Panels:
- /Path1
- /Marker1
- /Marker1/Topic1
+ - /RobotModel1
Splitter Ratio: 0.5
- Tree Height: 696
+ Tree Height: 481
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
@@ -82,7 +83,7 @@ Visualization Manager:
Enabled: true
Name: Marker
Namespaces:
- {}
+ "": true
Topic:
Depth: 5
Durability Policy: Volatile
@@ -91,9 +92,73 @@ Visualization Manager:
Reliability Policy: Reliable
Value: /simple_robot/visualization_marker
Value: true
+ - Alpha: 1
+ Class: rviz_default_plugins/RobotModel
+ Collision Enabled: false
+ Description File: ""
+ Description Source: Topic
+ Description Topic:
+ Depth: 5
+ Durability Policy: Volatile
+ History Policy: Keep Last
+ Reliability Policy: Reliable
+ Value: /robot_description
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ arm1:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ arm2:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ arm3:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ ee_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ map:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ Mass Properties:
+ Inertia: false
+ Mass: false
+ Name: RobotModel
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: true
+ - Class: rviz_default_plugins/TF
+ Enabled: false
+ Frame Timeout: 15
+ Frames:
+ All Enabled: true
+ Marker Scale: 1
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: false
+ Tree:
+ {}
+ Update Interval: 0
+ Value: false
Enabled: true
Global Options:
- Background Color: 48; 48; 48
+ Background Color: 61; 56; 70
Fixed Frame: map
Frame Rate: 30
Name: root
@@ -137,7 +202,7 @@ Visualization Manager:
Views:
Current:
Class: rviz_default_plugins/Orbit
- Distance: 8.930804252624512
+ Distance: 4.086756229400635
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
@@ -152,10 +217,10 @@ Visualization Manager:
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
- Pitch: 0.9647961854934692
+ Pitch: 1.149795651435852
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
- Yaw: 4.703580856323242
+ Yaw: 4.598585605621338
Saved: ~
Window Geometry:
Displays:
@@ -163,7 +228,7 @@ Window Geometry:
Height: 987
Hide Left Dock: false
Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd00000004000000000000015600000341fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000341000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000341fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000341000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007740000003efc0100000002fb0000000800540069006d00650100000000000007740000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005030000034100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ QMainWindow State: 000000ff00000000fd0000000400000000000001f7000002d9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000002d90000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f000002d9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e000002d90000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007740000005afc0100000002fb0000000800540069006d00650100000000000007740000058100fffffffb0000000800540069006d0065010000000000000450000000000000000000000406000002d900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
@@ -173,5 +238,5 @@ Window Geometry:
Views:
collapsed: false
Width: 1908
- X: -175
- Y: 52
+ X: 140
+ Y: 164
diff --git a/ros2_ws/src/simple_robot_2dof/setup.py b/ros2_ws/src/simple_robot_2dof/setup.py
index 656489b314929d2103cf3819485c9da96736d7be..347d70b0d118338a313e4b5eae5836a1be4082d0 100644
--- a/ros2_ws/src/simple_robot_2dof/setup.py
+++ b/ros2_ws/src/simple_robot_2dof/setup.py
@@ -1,3 +1,6 @@
+import os
+from glob import glob
+
from setuptools import find_packages, setup
package_name = 'simple_robot_2dof'
@@ -12,6 +15,10 @@ setup(
('share/' + package_name, ['package.xml']),
('share/' + package_name + '/launch', ['launch/kuka_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]*'))),
+
+
],
install_requires=['setuptools'],
zip_safe=True,
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 5f265da5894bb6430f938f8dc3a69b13d25552f4..eb5a94648a6f4f14a51a90f75a6fac476c2ff276 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
@@ -11,9 +11,7 @@ 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
class Forward_Kinematics_Node_2Dof(Node):
@@ -36,6 +34,15 @@ class Forward_Kinematics_Node_2Dof(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)
@@ -43,7 +50,7 @@ class Forward_Kinematics_Node_2Dof(Node):
self.angle_publisher_ = self.create_publisher(Float64, '/simple_robot/angle_2dof/plotjuggler', 10)
self.height_publisher_ = self.create_publisher(Float64, '/simple_robot/height_2dof/plotjuggler', 10)
# create a publisher for the pose
- self.pose_publisher_simple_robot_ = self.create_publisher(PoseStamped, '/simple_robot/end_effector_pose', 10)
+ self.pose_publisher_simple_robot_ = self.create_publisher(Pose, '/simple_robot/target_pose', 10)
self.pose_publisher_kuka_robot_ = self.create_publisher(Pose, '/kuka_iiwa/target_pose', 10)
#RViz
self.path = Path()
@@ -60,52 +67,63 @@ class Forward_Kinematics_Node_2Dof(Node):
self.index += 1
# call the transformation function
- self.transformation()
+ self.transformation_simple_robot()
- def transformation(self):
+ 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 transformation_simple_robot(self):
# Define the desired rotation
- theta = - pi / 1.8
+ 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)
# Definte the desired height
- h = 0.3
+ self.h = 0.05
#Split h
- h_fraction = self.phi * h
+ self.h_fraction = self.phi * self.h
- self.publish_height(h_fraction)
+ self.publish_height(self.h_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, h_fraction],
+ 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, self.h_fraction],
[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
+ self.simple_robot_joint_angles = np.zeros(len(self.simple_robot_Slist[0]))
- start_forward_kinematics = FKinBody(self.M, self.Blist, self.joint_angles)
+ start_forward_kinematics_simple = FKinBody(self.simple_robot_M, self.simple_robot_Blist, self.simple_robot_joint_angles)
- start_position = start_forward_kinematics[:4, 3]
- start_orientation = Rotation.from_matrix(start_forward_kinematics[:3, :3]).as_quat()
+ end_forward_kinematics_simple = np.matmul(self.intermediate_rotation, start_forward_kinematics_simple)
- end_forward_kinematics = np.matmul(intermediate_rotation, start_forward_kinematics)
+ end_position_simple = end_forward_kinematics_simple[:4, 3]
+ end_orientation_simple = Rotation.from_matrix(end_forward_kinematics_simple[:3, :3]).as_quat()
- 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_simple, end_orientation_simple)
- self.publish_pose_simple_robot(end_position, end_orientation)
+ self.construct_path(end_position_simple)
+
if self.index == 1:
- self.publish_pose_target_pose(end_position, end_orientation)
-
- self.construct_path(end_position)
+ self.transformation_kuka()
def construct_path(self, vector):
# RViz
@@ -160,19 +178,17 @@ class Forward_Kinematics_Node_2Dof(Node):
self.height_publisher_.publish(height_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_target_pose(self, vector, quaternion):
+ pose_msg_sr = Pose()
+ pose_msg_sr.position.x = vector[0]
+ pose_msg_sr.position.y = vector[1]
+ pose_msg_sr.position.z = vector[2]
+ pose_msg_sr.orientation.x = quaternion[0]
+ pose_msg_sr.orientation.y = quaternion[1]
+ pose_msg_sr.orientation.z = quaternion[2]
+ pose_msg_sr.orientation.w = quaternion[3]
+ self.pose_publisher_simple_robot_.publish(pose_msg_sr)
+
+ 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]
@@ -182,10 +198,7 @@ class Forward_Kinematics_Node_2Dof(Node):
pose_msg.orientation.y = quaternion[1]
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)
-
+ self.pose_publisher_kuka_robot_.publish(pose_msg)
def main(args = None):
diff --git a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/to.rviz b/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/to.rviz
deleted file mode 100644
index 3217ae54135c34c87987ade930dab8966b47fec3..0000000000000000000000000000000000000000
--- a/ros2_ws/src/simple_robot_2dof/simple_robot_2dof/to.rviz
+++ /dev/null
@@ -1,162 +0,0 @@
-Panels:
- - Class: rviz_common/Displays
- Help Height: 78
- Name: Displays
- Property Tree Widget:
- Expanded:
- - /Global Options1
- - /Status1
- - /Path1
- Splitter Ratio: 0.5
- Tree Height: 555
- - Class: rviz_common/Selection
- Name: Selection
- - Class: rviz_common/Tool Properties
- Expanded:
- - /2D Goal Pose1
- - /Publish Point1
- Name: Tool Properties
- Splitter Ratio: 0.5886790156364441
- - Class: rviz_common/Views
- Expanded:
- - /Current View1
- Name: Views
- Splitter Ratio: 0.5
- - Class: rviz_common/Time
- Experimental: false
- Name: Time
- SyncMode: 0
- SyncSource: ""
-Visualization Manager:
- Class: ""
- Displays:
- - Alpha: 0.5
- Cell Size: 1
- Class: rviz_default_plugins/Grid
- Color: 160; 160; 164
- Enabled: true
- Line Style:
- Line Width: 0.029999999329447746
- Value: Lines
- Name: Grid
- Normal Cell Count: 0
- Offset:
- X: 0
- Y: 0
- Z: 0
- Plane: XY
- Plane Cell Count: 10
- Reference Frame: <Fixed Frame>
- Value: true
- - Alpha: 1
- Buffer Length: 1
- Class: rviz_default_plugins/Path
- Color: 25; 255; 0
- Enabled: true
- Head Diameter: 0.30000001192092896
- Head Length: 0.20000000298023224
- Length: 0.30000001192092896
- Line Style: Lines
- Line Width: 0.029999999329447746
- Name: Path
- Offset:
- X: 0
- Y: 0
- Z: 0
- Pose Color: 255; 85; 255
- Pose Style: None
- Radius: 0.029999999329447746
- Shaft Diameter: 0.10000000149011612
- Shaft Length: 0.10000000149011612
- Topic:
- Depth: 5
- Durability Policy: Volatile
- Filter size: 10
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /visualization_path
- Value: true
- Enabled: true
- Global Options:
- Background Color: 48; 48; 48
- Fixed Frame: map
- Frame Rate: 30
- Name: root
- Tools:
- - Class: rviz_default_plugins/Interact
- Hide Inactive Objects: true
- - Class: rviz_default_plugins/MoveCamera
- - Class: rviz_default_plugins/Select
- - Class: rviz_default_plugins/FocusCamera
- - Class: rviz_default_plugins/Measure
- Line color: 128; 128; 0
- - Class: rviz_default_plugins/SetInitialPose
- Covariance x: 0.25
- Covariance y: 0.25
- Covariance yaw: 0.06853891909122467
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /initialpose
- - Class: rviz_default_plugins/SetGoal
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /goal_pose
- - Class: rviz_default_plugins/PublishPoint
- Single click: true
- Topic:
- Depth: 5
- Durability Policy: Volatile
- History Policy: Keep Last
- Reliability Policy: Reliable
- Value: /clicked_point
- Transformation:
- Current:
- Class: rviz_default_plugins/TF
- Value: true
- Views:
- Current:
- Class: rviz_default_plugins/Orbit
- Distance: 8.303964614868164
- Enable Stereo Rendering:
- Stereo Eye Separation: 0.05999999865889549
- Stereo Focal Distance: 1
- Swap Stereo Eyes: false
- Value: false
- Focal Point:
- X: 0
- Y: 0
- Z: 0
- Focal Shape Fixed Size: true
- Focal Shape Size: 0.05000000074505806
- Invert Z Axis: false
- Name: Current View
- Near Clip Distance: 0.009999999776482582
- Pitch: 0.5947965383529663
- Target Frame: <Fixed Frame>
- Value: Orbit (rviz)
- Yaw: 4.655405521392822
- Saved: ~
-Window Geometry:
- Displays:
- collapsed: false
- Height: 846
- Hide Left Dock: false
- Hide Right Dock: false
- QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
- Selection:
- collapsed: false
- Time:
- collapsed: false
- Tool Properties:
- collapsed: false
- Views:
- collapsed: false
- Width: 1200
- X: 28
- Y: 28
diff --git a/ros2_ws/src/simple_robot_2dof/urdf/simple_robot.urdf b/ros2_ws/src/simple_robot_2dof/urdf/simple_robot.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..22afcb72da5b9bbb8b848df53399c761387bba40
--- /dev/null
+++ b/ros2_ws/src/simple_robot_2dof/urdf/simple_robot.urdf
@@ -0,0 +1,104 @@
+<?xml version="1.0"?>
+<robot name="simple_robot">
+
+ <link name="map">
+ <visual>
+ <origin xyz="0 0 0.05" rpy="0 0 0"/>
+ <geometry>
+ <box size = "0.5 0.5 0.1"/>
+ </geometry>
+ <material name="grey">
+ <color rgba="0.5 0.5 0.5 1"/>
+ </material>
+ </visual>
+ </link>
+
+ <link name="arm1">
+ <visual>
+ <origin xyz="0 0 0.355" rpy="0 0 0"/>
+ <geometry>
+ <cylinder length="0.71" radius="0.02"/>
+ </geometry>
+ <material name="red">
+ <color rgba="0.75 0.0 0.0 1"/>
+ </material>
+ </visual>
+ <inertial>
+ <origin xyz="0 0 0.355" rpy="0 0 0"/>
+ <mass value="5"/>
+ <inertia ixx="0.05" ixy="0" ixz="0" iyy="0.05" iyz="0" izz="0.05"/>
+ </inertial>
+ </link>
+
+ <joint name="joint1" type="revolute">
+ <parent link="map"/>
+ <child link="arm1"/>
+ <origin xyz="0 0 0.05"/>
+ <axis xyz="0 0 1"/>
+ <limit effort = "300" lower="-2.96705972839" upper="2.96705972839" velocity = "10" />
+ </joint>
+
+ <link name="arm2">
+ <visual>
+ <origin xyz="-0.15 0 0" rpy="0 -1.5707 0"/>
+ <geometry>
+ <cylinder length="0.3" radius="0.02"/>
+ </geometry>
+ <material name="green">
+ <color rgba="0.0 0.75 0.0 1"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="5"/>
+ <inertia ixx="0.05" ixy="0" ixz="0" iyy="0.05" iyz="0" izz="0.05"/>
+ <origin xyz="-0.15 0 0" rpy="0 -1.5707 0"/>
+ </inertial>
+ </link>
+
+ <joint name="joint2" type="revolute">
+ <parent link="arm1"/>
+ <child link="arm2"/>
+ <origin xyz="0 0 0.71"/>
+ <axis xyz="0 1 0"/>
+ <limit effort = "300" lower="-2.96705972839" upper="2.96705972839" velocity = "10" />
+ </joint>
+
+ <link name="arm3">
+ <visual>
+ <origin xyz="-0.15 0 0" rpy="0 -1.5707 0"/>
+ <geometry>
+ <cylinder length="0.3" radius="0.02"/>
+ </geometry>
+ <material name="blue">
+ <color rgba="0.0 0.0 0.75 1"/>
+ </material>
+ </visual>
+ <inertial>
+ <mass value="5"/>
+ <inertia ixx="0.05" ixy="0" ixz="0" iyy="0.05" iyz="0" izz="0.05"/>
+ <origin xyz="-0.15 0 0" rpy="0 -1.5707 0"/>
+ </inertial>
+ </link>
+
+ <joint name="joint3" type="revolute">
+ <parent link="arm2"/>
+ <child link="arm3"/>
+ <origin xyz="-0.3 0 0"/>
+ <axis xyz="0 1 0"/>
+ <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>
+
+
+
+
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 2a491a6cd3982b1827fbac0cf9de472853a1631c..4f8b06e155af6d517dd537976dd8927e95b28f53 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
@@ -50,9 +50,7 @@ class InverseKinematicsSimpleRobot(Node):
eomg = 1e-4 # Fehlergrenze für Winkel
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)
-
+ joint_angles, success = IKinBody(self.Blist, self.M, T_sd, self.joint_angles, eomg, ev)
if success:
joint_state_msg = JointState()