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 dcd86a4512c5da509964ac1be6a3b8e2315f7db2..4c1b605ea40d27a2054ae612367db50df44724be 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/urdf/simple_robot.urdf b/ros2_ws/src/simple_robot_1dof/urdf/simple_robot.urdf
index 22afcb72da5b9bbb8b848df53399c761387bba40..9c63ad0059ea3972539c781a52f98c0e1383f511 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,16 @@
<?xml version="1.0"?>
<robot name="simple_robot">
- <link name="map">
+ <link name="map" />
+
+ <joint name = "map_simple_robot_joint" type = "fixed">
+ <origin xyz="0 0 0" rpy = "0 0 0" />
+ <parent link = "map" />
+ <child link = "base_link" />
+ </joint>
+
+
+ <link name="base_link">
<visual>
<origin xyz="0 0 0.05" rpy="0 0 0"/>
<geometry>
@@ -31,7 +40,7 @@
</link>
<joint name="joint1" type="revolute">
- <parent link="map"/>
+ <parent link="base_link"/>
<child link="arm1"/>
<origin xyz="0 0 0.05"/>
<axis xyz="0 0 1"/>
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 43b9271ba94adaed8433c04c0c7fc6542c5ebd8a..145b493fe00c9c9932a7cd157c5891aa8f4bd44b 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
@@ -124,6 +124,12 @@ def generate_launch_description():
{'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_1,
@@ -131,7 +137,8 @@ def generate_launch_description():
reference_node,
rviz_node,
inverse_kinematic_node,
- simple_robot_inverse_kinematics_node,
+ # joint_state_publisher_gui,
+ # simple_robot_inverse_kinematics_node,
robot_state_publisher
]
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 eb5a94648a6f4f14a51a90f75a6fac476c2ff276..78a2a3c8c20b671cc1aad2f31903f4bc77e5f0e1 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
@@ -8,17 +8,19 @@ from math import sin, cos, pi
import numpy as np
from std_msgs.msg import Float64
from nav_msgs.msg import Path
+from sensor_msgs.msg import JointState
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Pose
from visualization_msgs.msg import Marker
from scipy.spatial.transform import Rotation
+from rclpy.clock import Clock
class Forward_Kinematics_Node_2Dof(Node):
def __init__(self):
super().__init__('forward_kinematics_node_2dof')
- # Create a subscriber, initialising the transformation
+ # create a subscriber
self.subscription = self.create_subscription(
Float64,
'/dof2/reference_angles',
@@ -35,7 +37,7 @@ class Forward_Kinematics_Node_2Dof(Node):
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')
+ get_package_share_directory('simple_robot_2dof'), '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"]
@@ -50,7 +52,8 @@ 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(Pose, '/simple_robot/target_pose', 10)
+ self.joint_state_publisher_simple_robot_ = self.create_publisher(JointState, '/joint_states', 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()
@@ -95,7 +98,7 @@ class Forward_Kinematics_Node_2Dof(Node):
self.publish_angle(self.theta_fraction)
# Definte the desired height
- self.h = 0.05
+ self.h = 0.2
#Split h
self.h_fraction = self.phi * self.h
@@ -118,7 +121,8 @@ class Forward_Kinematics_Node_2Dof(Node):
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.publish_pose_simple_robot(end_position_simple, end_orientation_simple)
+ self.publish_joint_states_simple_robot()
self.construct_path(end_position_simple)
@@ -155,9 +159,9 @@ class Forward_Kinematics_Node_2Dof(Node):
marker.type = Marker.SPHERE
marker.action = Marker.ADD
marker.pose = current_pose
- marker.scale.x = 0.05
- marker.scale.y = 0.05
- marker.scale.z = 0.05
+ marker.scale.x = 0.025
+ marker.scale.y = 0.025
+ marker.scale.z = 0.025
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.0
@@ -177,16 +181,33 @@ class Forward_Kinematics_Node_2Dof(Node):
height_msg.data = height
self.height_publisher_.publish(height_msg)
- def publish_pose_simple_robot(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_simple_robot(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_joint_states_simple_robot(self):
+
+ joint_names = ['joint1', 'joint2', 'joint3', 'joint4']
+ joint_states = [0.0, 0.0, 0.0, 0.0]
+ joint_states[1] = self.h_fraction
+ joint_states[0] = self.theta_fraction
+
+ joint_state_msg = JointState()
+ joint_state_msg.header.stamp = Clock().now().to_msg()
+
+ joint_state_msg.name = joint_names
+ joint_state_msg.position = joint_states
+
+ self.joint_state_publisher_simple_robot_.publish(joint_state_msg)
+
+
def publish_pose_kuka_pose(self, vector, quaternion):
# Create a Pose message for the target pose
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 22afcb72da5b9bbb8b848df53399c761387bba40..f662cae3da49830c7886561f8391f836be920895 100644
--- a/ros2_ws/src/simple_robot_2dof/urdf/simple_robot.urdf
+++ b/ros2_ws/src/simple_robot_2dof/urdf/simple_robot.urdf
@@ -1,7 +1,15 @@
<?xml version="1.0"?>
<robot name="simple_robot">
- <link name="map">
+ <link name="map" />
+
+ <joint name = "map_simple_robot_joint" type = "fixed">
+ <origin xyz="0 0 0" rpy = "0 0 0" />
+ <parent link = "map" />
+ <child link = "base_link" />
+ </joint>
+
+ <link name="base_link">
<visual>
<origin xyz="0 0 0.05" rpy="0 0 0"/>
<geometry>
@@ -15,34 +23,34 @@
<link name="arm1">
<visual>
- <origin xyz="0 0 0.355" rpy="0 0 0"/>
+ <origin xyz="0 0 0.35" rpy="0 0 0"/>
<geometry>
- <cylinder length="0.71" radius="0.02"/>
+ <cylinder length="0.6" radius="0.019"/>
</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"/>
+ <origin xyz="0 0 0.2" 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"/>
+ <parent link="base_link"/>
<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" />
+ <limit effort = "300" lower="-0.1" upper="0.5" velocity = "10" />
</joint>
<link name="arm2">
<visual>
- <origin xyz="-0.15 0 0" rpy="0 -1.5707 0"/>
+ <origin xyz="0 0 0.155" rpy="0 0 0"/>
<geometry>
- <cylinder length="0.3" radius="0.02"/>
+ <cylinder length="0.31" radius="0.02"/>
</geometry>
<material name="green">
<color rgba="0.0 0.75 0.0 1"/>
@@ -51,16 +59,16 @@
<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"/>
+ <origin xyz="0 0 0.155" rpy="0 0 0"/>
</inertial>
</link>
- <joint name="joint2" type="revolute">
+ <joint name="joint2" type="prismatic">
<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" />
+ <origin xyz="0 0 0.45"/>
+ <axis xyz="0 0 1"/>
+ <limit effort = "300" lower="-2.1" upper="2.0" velocity = "10" />
</joint>
<link name="arm3">
@@ -83,15 +91,41 @@
<joint name="joint3" type="revolute">
<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">
+ <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.3 0 0" rpy="0 -1.5707 0"/>
+ </inertial>
+ </link>
+
+ <joint name="joint4" type="revolute">
+ <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>
+
<link name="ee_link" />
<joint name="ee_joint" type="fixed">
- <parent link="arm3"/>
+ <parent link="arm4"/>
<child link="ee_link"/>
<origin xyz="-0.3 0 0"/>
</joint>
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 4f8b06e155af6d517dd537976dd8927e95b28f53..47061e84123f418b136eae258aee4871e364971c 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
@@ -4,7 +4,7 @@ import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Pose
from sensor_msgs.msg import JointState
-from modern_robotics import IKinBody, RpToTrans, FKinBody
+from modern_robotics import IKinBody, RpToTrans, FKinBody, IKinSpace
import numpy as np
from ament_index_python.packages import get_package_share_directory
from mr_urdf_loader import loadURDF
@@ -32,10 +32,11 @@ class InverseKinematicsSimpleRobot(Node):
self.Glist = self.simple_robot["Glist"]
self.Blist = self.simple_robot["Blist"]
+
self.joint_names = [f'joint{i+1}' for i in range(len(self.Slist[0]))]
self.joint_angles = np.zeros(len(self.Slist[0]))
-
+
T = FKinBody(self.M, self.Blist, self.joint_angles)
rotation_matrix = T[:3, :3]
quat = Rotation.from_matrix(rotation_matrix).as_quat()
@@ -57,6 +58,7 @@ class InverseKinematicsSimpleRobot(Node):
joint_state_msg.header.stamp = Clock().now().to_msg()
joint_state_msg.name = self.joint_names
joint_state_msg.position = joint_angles.tolist()
+ self.get_logger().info('joint_angles "%s":' %joint_state_msg.position)
self.publisher.publish(joint_state_msg)
else: