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 2d68a091611c965f0e5d042f323101c2028f6165..aa89ec9a5ccd69f74d83750e2d72d44d14e87752 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
@@ -44,6 +44,8 @@ class Forward_Kinematics_Node_2Dof(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 = np.zeros(len(self.simple_robot_Slist[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)
@@ -71,23 +73,7 @@ class Forward_Kinematics_Node_2Dof(Node):
# call the transformation function
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):
# Define the desired rotation
@@ -112,15 +98,13 @@ class Forward_Kinematics_Node_2Dof(Node):
[0, 0, 0, 1]
])
- self.simple_robot_joint_angles = np.zeros(len(self.simple_robot_Slist[0]))
-
start_forward_kinematics_simple = FKinBody(self.simple_robot_M, self.simple_robot_Blist, self.simple_robot_joint_angles)
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()
-
+
+ # 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_joint_states_simple_robot()
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 712896fca936b67e3e8459e6cae9ca7a20ba7dd9..18b276f0fefe0a5c897a49beab5475725d9a0696 100644
--- a/ros2_ws/src/simple_robot_2dof/urdf/simple_robot.urdf
+++ b/ros2_ws/src/simple_robot_2dof/urdf/simple_robot.urdf
@@ -23,9 +23,9 @@
<link name="arm1">
<visual>
- <origin xyz="0 0 0.405" rpy="0 0 0"/>
+ <origin xyz="0 0 0.605" rpy="0 0 0"/>
<geometry>
- <cylinder length="0.71" radius="0.019"/>
+ <cylinder length="1.11" radius="0.019"/>
</geometry>
<material name="red">
<color rgba="0.75 0.0 0.0 1"/>
@@ -50,7 +50,7 @@
<visual>
<origin xyz="0 0 0.155" rpy="0 0 0"/>
<geometry>
- <cylinder length="0.31" radius="0.02"/>
+ <cylinder length="0.41" radius="0.02"/>
</geometry>
<material name="green">
<color rgba="0.0 0.75 0.0 1"/>
diff --git a/ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_launch.py b/ros2_ws/src/simple_robot_3dof/launch/simple_robot_rviz_simulation_3dof_launch.py
similarity index 82%
rename from ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_launch.py
rename to ros2_ws/src/simple_robot_3dof/launch/simple_robot_rviz_simulation_3dof_launch.py
index 02febc5994ce0ec9e1529637d6919b5f779ac269..90ccb37e67fa828499000fd6c4f933ee78ebbe30 100644
--- a/ros2_ws/src/simple_robot_3dof/launch/kuka_rviz_simulation_3dof_launch.py
+++ b/ros2_ws/src/simple_robot_3dof/launch/simple_robot_rviz_simulation_3dof_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 = []
)
+ robot_state_publisher = Node(
+ package='robot_state_publisher',
+ executable='robot_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_1,
forward_kinematics_node_2,
reference_node,
rviz_node,
- inverse_kinematics_node
+ inverse_kinematics_node,
+ robot_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_3dof/rviz/forward_kinematics_simulations.rviz b/ros2_ws/src/simple_robot_3dof/rviz/forward_kinematics_simulations.rviz
index afc67bf036d721c108218a5270687293194fe0b8..e6d557b9c250f68199af9b8ff83e0ce80a99277a 100644
--- a/ros2_ws/src/simple_robot_3dof/rviz/forward_kinematics_simulations.rviz
+++ b/ros2_ws/src/simple_robot_3dof/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
@@ -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: -161
- Y: 12
+ X: 140
+ Y: 164
diff --git a/ros2_ws/src/simple_robot_3dof/rviz/forward_kinematics_simulations.rviz:Zone.Identifier b/ros2_ws/src/simple_robot_3dof/rviz/forward_kinematics_simulations.rviz:Zone.Identifier
deleted file mode 100644
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..0000000000000000000000000000000000000000
diff --git a/ros2_ws/src/simple_robot_3dof/setup.py b/ros2_ws/src/simple_robot_3dof/setup.py
index 11f228e6e3048098ade71d2d3f86bfd7035b0363..0b173fd55f93e0c892a34d5ed88501d45c727077 100644
--- a/ros2_ws/src/simple_robot_3dof/setup.py
+++ b/ros2_ws/src/simple_robot_3dof/setup.py
@@ -1,4 +1,6 @@
from setuptools import find_packages, setup
+import os
+from glob import glob
package_name = 'simple_robot_3dof'
@@ -10,8 +12,10 @@ setup(
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
- ('share/' + package_name + '/launch', ['launch/kuka_rviz_simulation_3dof_launch.py']),
+ ('share/' + package_name + '/launch', ['launch/simple_robot_rviz_simulation_3dof_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_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 e9e1111ea88f710ddcadfed8a06db085212140ce..5c5bbe7bef524fd3f97d8bcbbe32a41a9388092a 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
@@ -8,12 +8,13 @@ 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
-import transformations
from scipy.spatial.transform import Rotation
-import time
+from rclpy.clock import Clock
+
class Forward_Kinematics_Node_3Dof(Node):
@@ -34,12 +35,30 @@ class Forward_Kinematics_Node_3Dof(Node):
self.Mlist = self.robot_model["Mlist"]
self.Glist = self.robot_model["Glist"]
self.Blist = self.robot_model["Blist"]
+ self.kuka_joint_angles = np.zeros(len(self.Slist[0]))
+ self.kuka_joint_angles[0] = np.pi
+ self.kuka_joint_angles[1] = np.pi/2
+ self.kuka_joint_angles[3] = np.pi/2
+ self.kuka_joint_angles[5] = np.pi/2
+
+ urdf_file_path_simple_robot = os.path.join(
+ get_package_share_directory('simple_robot_3dof'), '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"]
+ self.simple_robot_joint_angles = [0.0, 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)
self.point_publisher_ = self.create_publisher(Marker, '/simple_robot/visualization_marker', 10)
- self.pose_publisher_simple_robot_ = self.create_publisher(PoseStamped, '/simple_robot/end_effector_pose', 10)
+ self.joint_state_publisher_simple_robot_ = self.create_publisher(JointState, '/joint_states', 10)
+
+ # self.pose_publisher_simple_robot_ = self.create_publisher(PoseStamped, '/simple_robot/end_effector_pose', 10)
self.pose_publisher_kuka_robot_ = self.create_publisher(Pose, '/kuka_iiwa/target_pose', 10)
#RViz
self.path = Path()
@@ -55,53 +74,57 @@ class Forward_Kinematics_Node_3Dof(Node):
if self.phi == 0:
self.index += 1
# call the transformation function
- self.transformation()
+ self.transformation_simple_robot()
- def transformation(self):
+ def transformation_simple_robot(self):
# Define the desired rotation
- theta = - pi / 2
+ self.theta = - pi / 2
# Split theta
- theta_fraction = self.phi * theta
+ self.theta_fraction = self.phi * self.theta
# Definte the desired height
- h = 0.2
+ self.h = 0.2
#Split h
- h_fraction = self.phi * h
+ self.h_fraction = self.phi * self.h
# define the desired length
- d = 0.1
+ self.d = 0.1
# split d
- d_fraction = self.phi * d
+ self.d_fraction = self.phi * self.d
# Define the matrix
- intermediate_translation = np.array([
- [cos(theta_fraction), -sin(theta_fraction), 0, 0],
- [sin(theta_fraction), cos(theta_fraction), 0, d_fraction],
- [0, 0, 1, h_fraction],
+ self.intermediate_translation = np.array([
+ [cos(self.theta_fraction), -sin(self.theta_fraction), 0, 0],
+ [sin(self.theta_fraction), cos(self.theta_fraction), 0, self.d_fraction],
+ [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
+ start_forward_kinematics_sr = FKinBody(self.simple_robot_M, self.simple_robot_Blist, self.simple_robot_joint_angles)
- start_forward_kinematics = FKinBody(self.M, self.Blist, self.joint_angles)
+ end_forward_kinematics_sr = np.matmul(self.intermediate_translation, start_forward_kinematics_sr)
- 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_translation, start_forward_kinematics)
+ end_position_sr = end_forward_kinematics_sr[:4, 3]
- end_position = end_forward_kinematics[:4, 3]
- end_orientation = Rotation.from_matrix(end_forward_kinematics[:3, :3]).as_quat()
+ self.publish_joint_states_simple_robot()
- self.publish_pose_simple_robot(end_position, end_orientation)
+ # end_orientation_sr = Rotation.from_matrix(end_forward_kinematics_sr[:3, :3]).as_quat()
+ # self.publish_pose_simple_robot(end_position_sr, end_orientation_sr)
if self.index == 1:
- self.publish_pose_target_pose(end_position, end_orientation)
+ self.transformation_kuka()
+
+ self.construct_path(end_position_sr)
+
+ def transformation_kuka(self):
- self.construct_path(end_position)
+ start_forward_kinematics_kuka = FKinBody(self.M, self.Blist, self.kuka_joint_angles)
+
+ self.end_forward_kinematics_kuka_1 = np.matmul(self.intermediate_translation, start_forward_kinematics_kuka)
+
+ end_position_kuka_1 = self.end_forward_kinematics_kuka_1[:4, 3]
+ end_orientation_kuka_1 = Rotation.from_matrix(self.end_forward_kinematics_kuka_1[:3, :3]).as_quat()
+ self.publish_pose_kuka_pose(end_position_kuka_1, end_orientation_kuka_1)
def construct_path(self, vector):
# RViz
@@ -144,20 +167,36 @@ class Forward_Kinematics_Node_3Dof(Node):
# Publish the marker
self.point_publisher_.publish(marker)
- 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):
+ 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[0] = self.d_fraction
+ joint_states[1] = self.h_fraction
+ joint_states[2] = 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_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_kuka_pose(self, vector, quaternion):
# Create a Pose message for the target pose
pose_msg = Pose()
pose_msg.position.x = vector[0]
@@ -168,8 +207,6 @@ class Forward_Kinematics_Node_3Dof(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(1)
def main(args = None):
rclpy.init(args = args)
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 6d9737304951ea514061942162128418a5f1425a..030875b1570a1f48d0d42bae30f77e33662f4346 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
@@ -8,12 +8,12 @@ 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
-import transformations
from scipy.spatial.transform import Rotation
-import time
+from rclpy.clock import Clock
class Forward_Kinematics_Node_3Dof(Node):
@@ -35,6 +35,22 @@ class Forward_Kinematics_Node_3Dof(Node):
self.Mlist = self.robot_model["Mlist"]
self.Glist = self.robot_model["Glist"]
self.Blist = self.robot_model["Blist"]
+ self.kuka_joint_angles = np.zeros(len(self.Slist[0]))
+ self.kuka_joint_angles[0] = np.pi
+ self.kuka_joint_angles[1] = np.pi/2
+ self.kuka_joint_angles[3] = np.pi/2
+ self.kuka_joint_angles[5] = np.pi/2
+
+ urdf_file_path_simple_robot = os.path.join(
+ get_package_share_directory('simple_robot_3dof'), '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"]
+ self.simple_robot_joint_angles = [0.0, 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)
self.point_publisher_ = self.create_publisher(Marker, '/simple_robot/visualization_marker', 10)
@@ -43,7 +59,11 @@ class Forward_Kinematics_Node_3Dof(Node):
self.height_publisher_ = self.create_publisher(Float64, '/simple_robot/height_3dof/plotjuggler', 10)
self.length_publisher_ = self.create_publisher(Float64, '/simple_robot/length_3dof/plotjuggler', 10)
- self.pose_publisher_simple_robot_ = self.create_publisher(PoseStamped, '/simple_robot/end_effector_pose', 10)
+ self.joint_state_publisher_simple_robot_ = self.create_publisher(JointState, '/joint_states', 10)
+ self.theta_fraction = 0.0
+ self.h_fraction = 0.0
+ self.d_fraction = 0.0
+ # self.pose_publisher_simple_robot_ = self.create_publisher(PoseStamped, '/simple_robot/end_effector_pose', 10)
self.pose_publisher_kuka_robot_ = self.create_publisher(Pose, '/kuka_iiwa/target_pose', 10)
#RViz
self.path = Path()
@@ -61,94 +81,109 @@ class Forward_Kinematics_Node_3Dof(Node):
# check which function to call
if(self.index % 2 == 0):
# call the transformation function
- self.translation()
+ self.translation_simple_robot()
else:
- self.rotation()
-
-
- def rotation(self):
- # define the desired rotation
- theta = - pi / 2
- # 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]
- ])
-
- end_forward_kinematics_2 = np.matmul(intermediate_rotation, self.end_forward_kinematics_1)
+ self.rotation_simple_robot()
- end_position_2 = end_forward_kinematics_2[:4, 3]
- end_orientation_2 = Rotation.from_matrix(end_forward_kinematics_2[:3, :3]).as_quat()
- self.publish_pose_simple_robot(end_position_2, end_orientation_2)
- if self.index == 3:
- self.publish_pose_target_pose(end_position_2, end_orientation_2)
-
- # construct the path of the rotation
- self.construct_path(end_position_2)
-
- def translation(self):
+ def translation_simple_robot(self):
# definte the desired height
- h = 0.2
+ self.h = 0.25
# 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 desired length
- d = 0.22
+ self.d = 0.2
# split d
- d_fraction = self.phi * d
+ self.d_fraction = self.phi * self.d
+
+ self.theta_fraction = 0.0
- self.publish_length(d_fraction)
+ self.publish_length(self.d_fraction)
# define translation matrix
- intermediate_translation = np.array([
+ self.intermediate_translation = np.array([
[1, 0, 0, 0],
- [0, 1, 0, d_fraction],
- [0, 0, 1, h_fraction],
+ [0, 1, 0, self.d_fraction],
+ [0, 0, 1, self.h_fraction],
[0, 0, 0, 1]
])
- translation_matrix = np.array([
- [1, 0, 0, 0],
- [0, 1, 0, d],
- [0, 0, 1, h],
+ # translation_matrix = np.array([
+ # [1, 0, 0, 0],
+ # [0, 1, 0, self.d],
+ # [0, 0, 1, self.h],
+ # [0, 0, 0, 1]
+ # ])
+
+ start_forward_kinematics_1_sr = FKinBody(self.simple_robot_M, self.simple_robot_Blist, self.simple_robot_joint_angles)
+
+ self.end_forward_kinematics_1_sr = np.matmul(self.intermediate_translation, start_forward_kinematics_1_sr)
+
+ self.end_position_1_sr = self.end_forward_kinematics_1_sr[:4, 3]
+ # end_orientation_1_sr = Rotation.from_matrix(self.end_forward_kinematics_1[:3, :3]).as_quat()
+ # self.publish_pose_simple_robot(self.end_position_1_sr, end_orientation_1_sr)
+
+ self.publish_joint_states_simple_robot()
+ if self.index == 2:
+ self.kuka_translation()
+ # self.publish_pose_target_pose(end_position_1, end_orientation_1)
+
+ # construct the path of the translation
+ self.construct_path(self.end_position_1_sr)
+
+
+ def rotation_simple_robot(self):
+ # define the desired rotation
+ self.theta = - pi / 2
+ # split theta
+ self.theta_fraction = self.phi * self.theta
+
+ self.publish_angle(self.theta_fraction)
+
+ # define the matrix
+ 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.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
+ end_forward_kinematics_2 = np.matmul(self.intermediate_rotation, self.end_forward_kinematics_1_sr)
- start_forward_kinematics = FKinBody(self.M, self.Blist, self.joint_angles)
+ end_position_2_sr = end_forward_kinematics_2[:4, 3]
- start_position = start_forward_kinematics[:4, 3]
- start_orientation = Rotation.from_matrix(start_forward_kinematics[:3, :3]).as_quat()
+ # end_orientation_2_sr = Rotation.from_matrix(end_forward_kinematics_2[:3, :3]).as_quat()
+ # self.publish_pose_simple_robot(end_position_2, end_orientation_2)
- self.end_forward_kinematics_1 = np.matmul(intermediate_translation, start_forward_kinematics)
+ self.publish_joint_states_simple_robot()
+ if self.index == 3:
+ self.kuka_rotation()
+ # self.publish_pose_target_pose(end_position_2_sr, end_orientation_2_sr)
- end_position_1 = self.end_forward_kinematics_1[:4, 3]
- end_orientation_1 = Rotation.from_matrix(self.end_forward_kinematics_1[:3, :3]).as_quat()
+ # construct the path of the rotation
+ self.construct_path(end_position_2_sr)
- # calculate the end vector
- self.end_vector = np.matmul(translation_matrix, start_position)
+ def kuka_translation(self):
- self.publish_pose_simple_robot(end_position_1, end_orientation_1)
- if self.index == 2:
- self.publish_pose_target_pose(end_position_1, end_orientation_1)
+ start_forward_kinematics_kuka = FKinBody(self.M, self.Blist, self.kuka_joint_angles)
+ self.end_forward_kinematics_kuka_1 = np.matmul(self.intermediate_translation, start_forward_kinematics_kuka)
+
+ end_position_kuka_1 = self.end_forward_kinematics_kuka_1[:4, 3]
+ end_orientation_kuka_1 = Rotation.from_matrix(self.end_forward_kinematics_kuka_1[:3, :3]).as_quat()
+ self.publish_pose_kuka_pose(end_position_kuka_1, end_orientation_kuka_1)
+
+ def kuka_rotation(self):
+
+ end_forward_kinematics_kuka_2 = np.matmul(self.intermediate_rotation, self.end_forward_kinematics_kuka_1)
+
+ end_position_kuka_2 = end_forward_kinematics_kuka_2[:4, 3]
+ end_orientation_kuka_2 = Rotation.from_matrix(end_forward_kinematics_kuka_2[:3, :3]).as_quat()
+
+ self.publish_pose_kuka_pose(end_position_kuka_2, end_orientation_kuka_2)
- # construct the path of the translation
- self.construct_path(end_position_1)
def construct_path(self, vector):
# RViz
@@ -206,20 +241,36 @@ class Forward_Kinematics_Node_3Dof(Node):
length_msg.data = length
self.length_publisher_.publish(length_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):
+ # 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_joint_states_simple_robot(self):
+
+ joint_names = ['joint1', 'joint2', 'joint3', 'joint4']
+ joint_states = [0.0, 0.0, 0.0, 0.0]
+ joint_states[0] = self.d_fraction
+ joint_states[1] = self.h_fraction
+ joint_states[2] = 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
pose_msg = Pose()
pose_msg.position.x = vector[0]
@@ -230,8 +281,6 @@ class Forward_Kinematics_Node_3Dof(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(1)
def main(args = None):
diff --git a/ros2_ws/src/simple_robot_3dof/urdf/simple_robot.urdf b/ros2_ws/src/simple_robot_3dof/urdf/simple_robot.urdf
new file mode 100644
index 0000000000000000000000000000000000000000..2c70293d5a55cb2716f5710ff207b6d8961e0970
--- /dev/null
+++ b/ros2_ws/src/simple_robot_3dof/urdf/simple_robot.urdf
@@ -0,0 +1,138 @@
+<?xml version="1.0"?>
+<robot name="simple_robot">
+
+ <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>
+ <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.605" rpy="0 0 0"/>
+ <geometry>
+ <cylinder length="1.11" radius="0.019"/>
+ </geometry>
+ <material name="red">
+ <color rgba="0.75 0.0 0.0 1"/>
+ </material>
+ </visual>
+ <inertial>
+ <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="prismatic">
+ <parent link="base_link"/>
+ <child link="arm1"/>
+ <origin xyz="0 -0.20 0.05"/>
+ <axis xyz="0 1 0"/>
+ <limit effort = "300" lower="-0.1" upper="0.5" velocity = "10" />
+ </joint>
+
+ <link name="arm2">
+ <visual>
+ <origin xyz="0 0 0.155" rpy="0 0 0"/>
+ <geometry>
+ <cylinder length="0.41" 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 0 0.155" rpy="0 0 0"/>
+ </inertial>
+ </link>
+
+ <joint name="joint2" type="prismatic">
+ <parent link="arm1"/>
+ <child link="arm2"/>
+ <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">
+ <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 0 0.31"/>
+ <axis xyz="0 0 1"/>
+ <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="arm4"/>
+ <child link="ee_link"/>
+ <origin xyz="-0.3 0 0"/>
+ </joint>
+
+
+</robot>
+
+
+
+