Skip to content
Snippets Groups Projects
Commit 66109be0 authored by Ventsislav Dokusanski's avatar Ventsislav Dokusanski
Browse files

Fix urdf files and add 2dof simulation with the simple robot

parent 9effa8e5
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 110 additions and 37 deletions
ros2_ws/src/iiwa/iiwa_bringup/launch/__pycache__/iiwa.launch.cpython-310.pyc
+ 0
− 0
View file @ 66109be0
No preview for this file type
ros2_ws/src/simple_robot_1dof/urdf/simple_robot.urdf
+ 11
− 2
View file @ 66109be0
<?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"/>
......
ros2_ws/src/simple_robot_2dof/launch/kuka_rviz_simulation_2dof_launch.py
+ 8
− 1
View file @ 66109be0
......@@ -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
]
......
ros2_ws/src/simple_robot_2dof/simple_robot_2dof/forward_kinematics_node_2dof_1.py
+ 39
− 18
View file @ 66109be0
......@@ -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
......
ros2_ws/src/simple_robot_2dof/urdf/simple_robot.urdf
+ 48
− 14
View file @ 66109be0
<?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>
......
ros2_ws/src/simple_robot_inverse_kinematik/simple_robot_inverse_kinematik/inverse_kinematik.py
+ 4
− 2
View file @ 66109be0
......@@ -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:
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment

Help | Imprint | Privacy policy | Accessibility | Contact