From aa77823834e34fec3a82f5b7d02bc2d53f7b1e80 Mon Sep 17 00:00:00 2001
From: Ventsi <uquzq@student.kit.edu>
Date: Thu, 25 Jul 2024 18:56:42 +0200
Subject: [PATCH] Improve launch files and 1dof simulation
---
.../__pycache__/iiwa.launch.cpython-310.pyc | Bin 6424 -> 6424 bytes
.../config/initial_positions_IRS_Lab.yaml | 2 +-
.../inverse_kinematics_node.py | 2 +-
.../kuka_rviz_simulation_1dof_launch.py | 77 +++++++++++-------
.../forward_kinematics_node_1dof.py | 5 +-
.../simple_robot_1dof/reference_node_1dof.py | 32 ++++----
6 files changed, 69 insertions(+), 49 deletions(-)
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 5da1e78ccfbc8f2ed2a0f1fdab463779a5182290..2da6ee64e3d0230ee67e8f5e19f8e4191221ec33 100644
GIT binary patch
delta 20
bcmbPXG{cBHpO=@5fq{X6pMBv*ZeB?MErbKi
delta 20
bcmbPXG{cBHpO=@5fq{Wx_O<Sf+`N(iHN*uG
diff --git a/ros2_ws/src/iiwa/iiwa_description/config/initial_positions_IRS_Lab.yaml b/ros2_ws/src/iiwa/iiwa_description/config/initial_positions_IRS_Lab.yaml
index 120711a..06f74ba 100644
--- a/ros2_ws/src/iiwa/iiwa_description/config/initial_positions_IRS_Lab.yaml
+++ b/ros2_ws/src/iiwa/iiwa_description/config/initial_positions_IRS_Lab.yaml
@@ -1,6 +1,6 @@
# Default initial positions for the panda arm's ros2_control fake system
initial_positions:
- joint_a1: 0.16236
+ joint_a1: 2.97923
joint_a2: 1.42580
joint_a3: 0.02157
joint_a4: 1.64101
diff --git a/ros2_ws/src/iiwa/iiwa_inverse_kinematics/iiwa_inverse_kinematics/inverse_kinematics_node.py b/ros2_ws/src/iiwa/iiwa_inverse_kinematics/iiwa_inverse_kinematics/inverse_kinematics_node.py
index 7413c56..7fe6fa5 100644
--- a/ros2_ws/src/iiwa/iiwa_inverse_kinematics/iiwa_inverse_kinematics/inverse_kinematics_node.py
+++ b/ros2_ws/src/iiwa/iiwa_inverse_kinematics/iiwa_inverse_kinematics/inverse_kinematics_node.py
@@ -72,7 +72,7 @@ class InverseKinematicsNode(Node):
point = JointTrajectoryPoint()
point.positions = joint_angles.tolist()
# point.time_from_start.sec = 1 # Example time, adjust as needed
- point.time_from_start.nanosec = 100000000
+ point.time_from_start.nanosec = 450000000 # Balint fragen, ob das verbessert werden kann
joint_trajectory_msg.points.append(point)
self.publisher.publish(joint_trajectory_msg)
diff --git a/ros2_ws/src/simple_robot_1dof/launch/kuka_rviz_simulation_1dof_launch.py b/ros2_ws/src/simple_robot_1dof/launch/kuka_rviz_simulation_1dof_launch.py
index 57b00f9..ce6d589 100644
--- a/ros2_ws/src/simple_robot_1dof/launch/kuka_rviz_simulation_1dof_launch.py
+++ b/ros2_ws/src/simple_robot_1dof/launch/kuka_rviz_simulation_1dof_launch.py
@@ -16,65 +16,84 @@ def generate_launch_description():
'forward_kinematics_simulations.rviz'
)
- velocity_launch_arg = DeclareLaunchArgument('velocity', default_value = '0.015')
- velocity = LaunchConfiguration('velocity')
+ declared_arguments = []
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ 'max_velocity',
+ default_value = '0.015',
+ )
+ )
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ 'acceleration',
+ default_value = '0.005',
+ )
+ )
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ 'decelaration',
+ default_value = '0.005',
+ )
+ )
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ 'velocity_constant',
+ default_value = '0.015',
+ )
+ )
+ declared_arguments.append(
+ DeclareLaunchArgument(
+ 'constant_speed',
+ default_value = 'False',
+ )
+ )
- acceleration_launch_arg = DeclareLaunchArgument('acceleration', default_value = '0.0004')
+ max_velocity = LaunchConfiguration('max_velocity')
acceleration = LaunchConfiguration('acceleration')
-
- decelaration_launch_arg = DeclareLaunchArgument('decelaration', default_value = '0.0008')
decelaration = LaunchConfiguration('decelaration')
-
- slope_launch_arg = DeclareLaunchArgument('slope', default_value = '0.25') # [0 ; 0.5]
- slope = LaunchConfiguration('slope')
-
- velocity_constant_launch_arg = DeclareLaunchArgument('velocity_constant', default_value = '0.015')
velocity_constant = LaunchConfiguration('velocity_constant')
-
- constant_speed_launch_arg = DeclareLaunchArgument('constant_speed', default_value = 'False') # change
constant_speed = LaunchConfiguration('constant_speed')
- return LaunchDescription([
- velocity_launch_arg,
- acceleration_launch_arg,
- decelaration_launch_arg,
- slope_launch_arg,
- velocity_constant_launch_arg,
- constant_speed_launch_arg,
- Node(
+ forward_kinematics_node = Node(
package = package_name,
executable = 'forward_kinematics_node_1dof',
name = 'forward_kinematics_node',
output = 'screen',
parameters = []
- ),
- Node(
+ )
+ reference_node = Node(
package = package_name,
executable = 'reference_node_1dof',
name = 'reference_node',
output = 'screen',
parameters = [
- {'velocity': velocity},
+ {'max_velocity': max_velocity},
{'acceleration': acceleration},
{'decelaration': decelaration},
{'velocity_constant': velocity_constant},
- {'slope': slope},
{'constant_speed': constant_speed}
]
- ),
- Node(
+ )
+ rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', rviz_config_file]
- ),
- Node(
+ )
+ inverse_kinematics_node = Node(
package= 'iiwa_inverse_kinematics',
executable='inverse_kinematics_node',
name='inverse_kinematics',
output='screen',
parameters = []
)
- ])
\ No newline at end of file
+ nodes = [
+ forward_kinematics_node,
+ reference_node,
+ rviz_node,
+ inverse_kinematics_node
+ ]
+
+ return LaunchDescription(declared_arguments + nodes)
\ No newline at end of file
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 2e8b6d2..b36b3e8 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
@@ -61,7 +61,7 @@ class Forward_Kinematics_Node_1Dof(Node):
def transformation(self):
# Define the desired rotation here
- theta = pi / 1.8
+ theta = - pi / 1.8
# Split theta
theta_fraction = self.phi * theta
@@ -76,6 +76,7 @@ class Forward_Kinematics_Node_1Dof(Node):
])
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
@@ -93,7 +94,7 @@ class Forward_Kinematics_Node_1Dof(Node):
self.publish_pose_simple_robot(end_position, end_orientation)
- if self.index == 1:
+ if self.index == 2:
self.publish_pose_target_pose(end_position, end_orientation)
self.construct_path(end_position)
diff --git a/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/reference_node_1dof.py b/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/reference_node_1dof.py
index 785aaee..6fa9d3e 100644
--- a/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/reference_node_1dof.py
+++ b/ros2_ws/src/simple_robot_1dof/simple_robot_1dof/reference_node_1dof.py
@@ -13,24 +13,23 @@ class Reference_Node_1DoF(Node):
#Create a publisher sending the vector as a message
self.publisher_ = self.create_publisher(Float64, '/dof1/reference_angles', 10)
- self.declare_parameter('velocity', 0.0)
+ self.declare_parameter('max_velocity', 0.0)
self.declare_parameter('acceleration', 0.0)
self.declare_parameter('decelaration', 0.0)
self.declare_parameter('velocity_constant', 0.0)
- self.declare_parameter('slope', 0.0)
self.declare_parameter('constant_speed', False)
- self.velocity = self.get_parameter('velocity').get_parameter_value().double_value
+ self.max_velocity = self.get_parameter('max_velocity').get_parameter_value().double_value
self.acceleration = self.get_parameter('acceleration').get_parameter_value().double_value
self.decelaration = self.get_parameter('decelaration').get_parameter_value().double_value
self.velocity_constant = self.get_parameter('velocity_constant').get_parameter_value().double_value
- self.slope = self.get_parameter('slope').get_parameter_value().double_value
self.constant_speed = self.get_parameter('constant_speed').get_parameter_value().bool_value
+
self.phi_start = 0.0
self.phi_end = 1.0
self.phi = self.phi_start
- self.velocity_start = self.velocity
+ self.current_velocity = 0.0
self.timer_period = 0.1 # seconds
self.timer = self.create_timer(self.timer_period, self.ref_point_publisher)
@@ -42,22 +41,23 @@ class Reference_Node_1DoF(Node):
msg = Float64()
- if self.constant_speed == True:
+ if self.constant_speed:
self.phi = self.phi + self.velocity_constant
- elif self.constant_speed == False:
- if self.phi < self.slope:
- self.phi = self.phi + self.velocity
- self.velocity = self.velocity + self.acceleration
- elif self.phi > 1 - self.slope:
- self.phi = self.phi + self.velocity
- self.velocity = self.velocity - self.decelaration
+ else:
+ if self.current_velocity < self.max_velocity and self.phi < 0.5:
+ self.phi = self.phi + self.current_velocity
+ self.current_velocity = self.current_velocity + self.timer_period * self.acceleration
+ self.transition_punkt = self.phi
+ elif self.phi > 1 - self.transition_punkt:
+ self.phi = self.phi + self.current_velocity
+ self.current_velocity = self.current_velocity - self.timer_period * self.decelaration
else:
- self.phi = self.phi + self.velocity
+ self.phi = self.phi + self.current_velocity
#setting back to 0
if self.phi>self.phi_end:
self.phi = self.phi_start
- self.velocity = self.velocity_start
+ self.current_velocity = 0.0
# self.get_logger().info('Resetting "%f"' % self.phi)
msg.data = self.phi
@@ -72,4 +72,4 @@ def main(args = None):
rclpy.shutdown()
if __name__ == '__main__':
- main()
\ No newline at end of file
+ main()
--
GitLab