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 8b89fb919e669c60e8c24b56441b3d48c45e0b24..2bf529b2c7cb67249cdbf70b61b64cc645ef2778 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 8bb289ec7c12df87514606e101221bb60c9692d2..04b91614fe37588abacf252baacf290485ae5f4f 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 9c63ad0059ea3972539c781a52f98c0e1383f511..54b14680c64fbef832909f9456ca296b5a8cff24 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 aa89ec9a5ccd69f74d83750e2d72d44d14e87752..7cdbb44857bc6861d42ac8db580a3a855b63b778 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 18b276f0fefe0a5c897a49beab5475725d9a0696..f6dc71d854bfa5254b48a2e6e349c4130da39a67 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 5c5bbe7bef524fd3f97d8bcbbe32a41a9388092a..ea7f6fd14320adfe618388bd902b37b233b6ac77 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 030875b1570a1f48d0d42bae30f77e33662f4346..ffa832c5d78309b34dea201cee771a8377cc7613 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 2c70293d5a55cb2716f5710ff207b6d8961e0970..447d31580c4427ad444c406bf2a69ed6c6b17f7a 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>