From dfbdcf5e8ac9d920ff4865771b0544c556c95c74 Mon Sep 17 00:00:00 2001
From: Ventsi <uquzq@student.kit.edu>
Date: Sat, 17 Aug 2024 16:12:49 +0200
Subject: [PATCH] Add bed in URDF file of the simple robot and improve
simulations
---
...imple_robot_rviz_simulation_1dof_launch.py | 5 ++-
.../forward_kinematics_node_1dof.py | 39 ++++++++++---------
.../simple_robot_1dof/urdf/simple_robot.urdf | 29 ++++++++------
.../forward_kinematics_node_2dof_1.py | 4 +-
.../simple_robot_2dof/urdf/simple_robot.urdf | 17 +++++---
.../forward_kinematics_node_3dof_1.py | 6 +--
.../forward_kinematics_node_3dof_2.py | 6 +--
.../simple_robot_3dof/urdf/simple_robot.urdf | 13 +++++--
8 files changed, 71 insertions(+), 48 deletions(-)
diff --git a/ros2_ws/src/simple_robot_1dof/launch/simple_robot_rviz_simulation_1dof_launch.py b/ros2_ws/src/simple_robot_1dof/launch/simple_robot_rviz_simulation_1dof_launch.py
index 8b89fb9..2bf529b 100644
--- a/ros2_ws/src/simple_robot_1dof/launch/simple_robot_rviz_simulation_1dof_launch.py
+++ b/ros2_ws/src/simple_robot_1dof/launch/simple_robot_rviz_simulation_1dof_launch.py
@@ -21,6 +21,8 @@ def generate_launch_description():
with open(urdf_file, 'r') as infp:
robot_desc = infp.read()
+
+
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
@@ -105,7 +107,8 @@ def generate_launch_description():
executable='robot_state_publisher',
output='both',
parameters=[
- {'robot_description': robot_desc}
+ # {'robot_description': combined_robot_desc},
+ {'robot_description': robot_desc},
]
)
nodes = [
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 8bb289e..04b9161 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
@@ -67,24 +67,6 @@ class Forward_Kinematics_Node_1Dof(Node):
self.index += 1
self.transformation_simple_robot()
-
- 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):
@@ -120,6 +102,27 @@ class Forward_Kinematics_Node_1Dof(Node):
if self.index == 2:
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
pose = PoseStamped()
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 9c63ad0..54b1468 100644
--- a/ros2_ws/src/simple_robot_1dof/urdf/simple_robot.urdf
+++ b/ros2_ws/src/simple_robot_1dof/urdf/simple_robot.urdf
@@ -20,6 +20,15 @@
<color rgba="0.5 0.5 0.5 1"/>
</material>
</visual>
+ <visual>
+ <origin xyz="-0.7 0.55 0.235" rpy="0 0 0"/>
+ <geometry>
+ <box size = "0.9 1.9 0.47"/>
+ </geometry>
+ <material name="grey">
+ <color rgba="0.5 0.5 0.5 1"/>
+ </material>
+ </visual>
</link>
<link name="arm1">
@@ -28,8 +37,8 @@
<geometry>
<cylinder length="0.71" radius="0.02"/>
</geometry>
- <material name="red">
- <color rgba="0.75 0.0 0.0 1"/>
+ <material name="green">
+ <color rgba="0.0 0.75 0.0 1"/>
</material>
</visual>
<inertial>
@@ -53,8 +62,8 @@
<geometry>
<cylinder length="0.3" radius="0.02"/>
</geometry>
- <material name="green">
- <color rgba="0.0 0.75 0.0 1"/>
+ <material name="red">
+ <color rgba="0.75 0.0 0.0 1"/>
</material>
</visual>
<inertial>
@@ -64,12 +73,10 @@
</inertial>
</link>
- <joint name="joint2" type="revolute">
+ <joint name="joint2" type="fixed">
<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">
@@ -78,8 +85,8 @@
<geometry>
<cylinder length="0.3" radius="0.02"/>
</geometry>
- <material name="blue">
- <color rgba="0.0 0.0 0.75 1"/>
+ <material name="red">
+ <color rgba="0.75 0.0 0.0 1"/>
</material>
</visual>
<inertial>
@@ -89,12 +96,10 @@
</inertial>
</link>
- <joint name="joint3" type="revolute">
+ <joint name="joint3" type="fixed">
<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" />
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 aa89ec9..7cdbb44 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
@@ -195,8 +195,8 @@ class Forward_Kinematics_Node_2Dof(Node):
def publish_joint_states_simple_robot(self):
- joint_names = ['joint1', 'joint2', 'joint3', 'joint4']
- joint_states = [0.0, 0.0, 0.0, 0.0]
+ joint_names = ['joint1', 'joint2']
+ joint_states = [0.0, 0.0]
joint_states[1] = self.h_fraction
joint_states[0] = self.theta_fraction
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 18b276f..f6dc71d 100644
--- a/ros2_ws/src/simple_robot_2dof/urdf/simple_robot.urdf
+++ b/ros2_ws/src/simple_robot_2dof/urdf/simple_robot.urdf
@@ -19,6 +19,15 @@
<color rgba="0.5 0.5 0.5 1"/>
</material>
</visual>
+ <visual>
+ <origin xyz="-0.7 0.55 0.235" rpy="0 0 0"/>
+ <geometry>
+ <box size = "0.9 1.9 0.47"/>
+ </geometry>
+ <material name="grey">
+ <color rgba="0.5 0.5 0.5 1"/>
+ </material>
+ </visual>
</link>
<link name="arm1">
@@ -88,12 +97,10 @@
</inertial>
</link>
- <joint name="joint3" type="revolute">
+ <joint name="joint3" type="fixed">
<parent link="arm2"/>
<child link="arm3"/>
<origin xyz="0 0 0.31"/>
- <axis xyz="0 1 0"/>
- <limit effort = "300" lower="-2.96705972839" upper="2.96705972839" velocity = "10" />
</joint>
<link name="arm4">
@@ -113,12 +120,10 @@
</inertial>
</link>
- <joint name="joint4" type="revolute">
+ <joint name="joint4" type="fixed">
<parent link="arm3"/>
<child link="arm4"/>
<origin xyz="-0.3 0 0"/>
- <axis xyz="0 1 0"/>
- <limit effort = "300" lower="-2.96705972839" upper="2.96705972839" velocity = "10" />
</joint>
diff --git a/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_1.py b/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_1.py
index 5c5bbe7..ea7f6fd 100644
--- a/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_1.py
+++ b/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_1.py
@@ -49,7 +49,7 @@ class Forward_Kinematics_Node_3Dof(Node):
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 = [0.0, 0.0, 0.0, 0.0]
+ self.simple_robot_joint_angles = [0.0, 0.0, 0.0]
# Create two publishers for the RViz simulation - one for the whole path and one for the marker
@@ -169,8 +169,8 @@ class Forward_Kinematics_Node_3Dof(Node):
def publish_joint_states_simple_robot(self):
- joint_names = ['joint1', 'joint2', 'joint3', 'joint4']
- joint_states = [0.0, 0.0, 0.0, 0.0]
+ joint_names = ['joint1', 'joint2', 'joint3']
+ joint_states = [0.0, 0.0, 0.0]
joint_states[0] = self.d_fraction
joint_states[1] = self.h_fraction
joint_states[2] = self.theta_fraction
diff --git a/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_2.py b/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_2.py
index 030875b..ffa832c 100644
--- a/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_2.py
+++ b/ros2_ws/src/simple_robot_3dof/simple_robot_3dof/forward_kinematics_node_3dof_2.py
@@ -49,7 +49,7 @@ class Forward_Kinematics_Node_3Dof(Node):
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 = [0.0, 0.0, 0.0, 0.0]
+ self.simple_robot_joint_angles = [0.0, 0.0, 0.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)
@@ -256,8 +256,8 @@ class Forward_Kinematics_Node_3Dof(Node):
def publish_joint_states_simple_robot(self):
- joint_names = ['joint1', 'joint2', 'joint3', 'joint4']
- joint_states = [0.0, 0.0, 0.0, 0.0]
+ joint_names = ['joint1', 'joint2', 'joint3']
+ joint_states = [0.0, 0.0, 0.0]
joint_states[0] = self.d_fraction
joint_states[1] = self.h_fraction
joint_states[2] = self.theta_fraction
diff --git a/ros2_ws/src/simple_robot_3dof/urdf/simple_robot.urdf b/ros2_ws/src/simple_robot_3dof/urdf/simple_robot.urdf
index 2c70293..447d315 100644
--- a/ros2_ws/src/simple_robot_3dof/urdf/simple_robot.urdf
+++ b/ros2_ws/src/simple_robot_3dof/urdf/simple_robot.urdf
@@ -19,6 +19,15 @@
<color rgba="0.5 0.5 0.5 1"/>
</material>
</visual>
+ <visual>
+ <origin xyz="-0.7 0.45 0.235" rpy="0 0 0"/>
+ <geometry>
+ <box size = "0.9 1.9 0.47"/>
+ </geometry>
+ <material name="grey">
+ <color rgba="0.5 0.5 0.5 1"/>
+ </material>
+ </visual>
</link>
<link name="arm1">
@@ -113,12 +122,10 @@
</inertial>
</link>
- <joint name="joint4" type="revolute">
+ <joint name="joint4" type="fixed">
<parent link="arm3"/>
<child link="arm4"/>
<origin xyz="-0.3 0 0"/>
- <axis xyz="0 1 0"/>
- <limit effort = "300" lower="-2.96705972839" upper="2.96705972839" velocity = "10" />
</joint>
--
GitLab