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 2bf529b2c7cb67249cdbf70b61b64cc645ef2778..8f91fe7234b2652c0d219c9032c49bca9ba792a1 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
@@ -27,25 +27,25 @@ def generate_launch_description():
declared_arguments.append(
DeclareLaunchArgument(
'max_velocity',
- default_value = '0.015',
+ default_value = '0.01725',
)
)
declared_arguments.append(
DeclareLaunchArgument(
'acceleration',
- default_value = '0.005',
+ default_value = '0.001327',
)
)
declared_arguments.append(
DeclareLaunchArgument(
'decelaration',
- default_value = '0.005',
+ default_value = '0.001327',
)
)
declared_arguments.append(
DeclareLaunchArgument(
'velocity_constant',
- default_value = '0.015',
+ default_value = '0.014',
)
)
declared_arguments.append(
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 7c6eda29fb328e0fbba2c229ccace481b242dd93..fb4e2113d77cfaeae2c1732cede7230d5d13ff9c 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
@@ -18,13 +18,13 @@ class Forward_Kinematics_Node_1Dof(Node):
def __init__(self):
super().__init__('forward_kinematics_node_1dof')
- # Create a subscriber, initialising the transformation
self.subscription = self.create_subscription(
Float64,
'/dof1/reference_angles',
self.listener_callback,
10)
+ # load KUKA urdf
urdf_file_path = os.path.join(
get_package_share_directory('iiwa_description'), 'urdf', 'iiwa14.urdf')
self.robot_model = loadURDF(urdf_file_path)
@@ -34,6 +34,7 @@ class Forward_Kinematics_Node_1Dof(Node):
self.Glist = self.robot_model["Glist"]
self.Blist = self.robot_model["Blist"]
+ # load simple robot URDF
urdf_file_path_simple_robot = os.path.join(
get_package_share_directory('simple_robot_1dof'), 'urdf', 'simple_robot.urdf')
self.simple_robot_model = loadURDF(urdf_file_path_simple_robot)
@@ -43,15 +44,13 @@ class Forward_Kinematics_Node_1Dof(Node):
self.simple_robot_Glist = self.simple_robot_model["Glist"]
self.simple_robot_Blist = self.simple_robot_model["Blist"]
-
- # create two publishers for the RViz simulation - one for the whole path and one for the marker
+ # publishers
self.path_publisher_ = self.create_publisher(Path, '/simple_robot/visualization_path', 10)
self.point_publisher_ = self.create_publisher(Marker, '/simple_robot/visualization_marker', 10)
- # create a publisher for the plotjuggler angles
- self.angle_publisher_ = self.create_publisher(Float64, '/simple_robot/angles_1dof/plotjuggler', 10)
- # create a publisher for the pose
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)
+ self.velocity_pub = self.create_publisher(Float64, '/velocity', 10)
+
#RViz
self.path = Path()
self.path.header.frame_id = "map"
@@ -59,58 +58,58 @@ class Forward_Kinematics_Node_1Dof(Node):
self.index = 0
+
def listener_callback(self, msg):
# Get the message
- self.phi = msg.data
- # call the transformation function
- if self.phi == 0:
+ self.k = msg.data
+ # add 1 to index
+ if self.k == 0:
self.index += 1
+ # call the transformation function
self.transformation_simple_robot()
-
+
def transformation_simple_robot(self):
- # Define the desired rotation
+ # define the desired rotation
self.theta = - pi / 1.8
- # Split theta
- self.theta_fraction = self.phi * self.theta
-
- self.publish_angle(self.theta_fraction)
+ # split theta for interpolation
+ self.theta_fraction = self.k * self.theta
- # 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]
])
-
+ # define a list of 0s
self.simple_robot_joint_angles = np.zeros(len(self.simple_robot_Slist[0]))
-
+ # forward kinematics
start_forward_kinematics_simple = FKinBody(self.simple_robot_M, self.simple_robot_Blist, self.simple_robot_joint_angles)
+ # apply the transformation
end_forward_kinematics_simple = np.matmul(self.intermediate_rotation, start_forward_kinematics_simple)
-
+ # get the desired position and orientation
end_position_simple = end_forward_kinematics_simple[:4, 3]
end_orientation_simple = Rotation.from_matrix(end_forward_kinematics_simple[:3, :3]).as_quat()
-
-
+ # call the funct. publishing the desired pose
self.publish_pose_simple_robot(end_position_simple, end_orientation_simple)
-
+ # construct path
self.construct_path(end_position_simple)
+ # move KUKA robot only once, when index equals 2 and let simulation with simple robot continue
if self.index == 2:
self.transformation_kuka()
def transformation_kuka(self):
-
+ # list of joint states that may satisfy the desired pose
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
-
+ # analog to simple robot
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)
@@ -118,7 +117,6 @@ class Forward_Kinematics_Node_1Dof(Node):
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)
@@ -134,15 +132,13 @@ class Forward_Kinematics_Node_1Dof(Node):
pose.pose.orientation.w = 1.0
# if the path is completed clear it
- if self.phi == 0:
+ if self.k == 0:
self.path.poses.clear()
else:
self.path.poses.append(pose)
- # publish the path
self.path_publisher_.publish(self.path)
- # call the function publishing the marker
self.publish_marker(pose.pose)
def publish_marker(self, current_pose):
@@ -161,16 +157,9 @@ class Forward_Kinematics_Node_1Dof(Node):
marker.color.g = 0.75
marker.color.b = 0.0
- # publish the marker
self.point_publisher_.publish(marker)
- def publish_angle(self, angle):
- angle_msg = Float64()
- angle_msg.data = angle * 180 / pi
- self.angle_publisher_.publish(angle_msg)
-
-
def publish_pose_simple_robot(self, vector, quaternion):
pose_msg_sr = Pose()
pose_msg_sr.position.x = vector[0]
@@ -183,7 +172,6 @@ class Forward_Kinematics_Node_1Dof(Node):
self.pose_publisher_simple_robot_.publish(pose_msg_sr)
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]
pose_msg.position.y = vector[1]
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 a62a3e1abb4746b6aba7b71bd93323ee6587348d..a0418594c2659721d4d250011aff28969075adcf 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
@@ -10,9 +10,11 @@ class Reference_Node_1DoF(Node):
def __init__(self):
super().__init__('reference_node_1dof')
- #Create a publisher sending the vector as a message
+ # publishers
self.publisher_ = self.create_publisher(Float64, '/dof1/reference_angles', 10)
+ self.delta_publisher_ = self.create_publisher(Float64, '/delta', 10)
+ # parameters
self.declare_parameter('max_velocity', 0.0)
self.declare_parameter('acceleration', 0.0)
self.declare_parameter('decelaration', 0.0)
@@ -25,12 +27,12 @@ class Reference_Node_1DoF(Node):
self.velocity_constant = self.get_parameter('velocity_constant').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.index = 0
+ self.k_start = 0.0
+ self.k_end = 1.0
+ self.k = self.k_start
self.current_velocity = 0.0
-
+ self.faktor = 1.3
self.timer_period = 0.1 # seconds
self.timer = self.create_timer(self.timer_period, self.ref_point_publisher)
# List the points
@@ -41,26 +43,32 @@ class Reference_Node_1DoF(Node):
msg = Float64()
+ # const. speed
if self.constant_speed:
- self.phi = self.phi + self.velocity_constant
+ self.k = self.k + self.velocity_constant
+ # trapez. velocity
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_point = self.phi
- elif self.phi > 1 - self.transition_point:
- self.phi = self.phi + self.current_velocity
- self.current_velocity = self.current_velocity - self.timer_period * self.decelaration
+ # acceleration phase
+ if self.current_velocity < self.max_velocity and self.k < 0.5:
+ self.k = self.k + self.current_velocity
+ self.current_velocity = self.current_velocity + self.acceleration
+ self.transition_point = self.k
+ # constant velocity
+ elif self.k >= self.k_end - self.faktor * self.transition_point:
+ self.k = self.k + self.current_velocity
+ self.current_velocity = self.current_velocity - self.decelaration
+ # decelaration phase
else:
- self.phi = self.phi + self.current_velocity
+ self.current_velocity = self.max_velocity
+ self.k = self.k + self.current_velocity
#setting back to 0
- if self.phi>self.phi_end:
- self.phi = self.phi_start
+ if self.k>self.k_end:
+ self.k = self.k_start
self.current_velocity = 0.0
- # self.get_logger().info('Resetting "%f"' % self.phi)
+ self.index += 1
- msg.data = self.phi
+ msg.data = self.k
self.publisher_.publish(msg)