Skip to content

Commit 01eabbd

Browse files
Jepear19Jepear19
committed
Add ur3 to sausage exercise and add HAL sausage exercise repo
1 parent c85f67f commit 01eabbd

5 files changed

Lines changed: 1632 additions & 0 deletions

File tree

‎database/exercises/db.sql‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -308,6 +308,7 @@ COPY public.exercises_tools (id, exercise_id, tool_id) FROM stdin;
308308
81 19 video
309309
82 27 simulator
310310
83 27 console
311+
84 27 rviz
311312
\.
312313

313314
--

‎exercises/conveyor_exercise/python_template/Frequency.py‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
import time
2+
from datetime import datetime
3+
4+
start_time_internal_freq_control = None
5+
6+
7+
def tick(ideal_cycle: int = 50):
8+
global start_time_internal_freq_control
9+
finish_time = datetime.now()
10+
ideal_ms = 1000 / ideal_cycle
11+
12+
if start_time_internal_freq_control is None:
13+
start_time_internal_freq_control = finish_time
14+
return
15+
16+
dt = finish_time - start_time_internal_freq_control
17+
ms = (dt.days * 24 * 60 * 60 + dt.seconds) * 1000 + dt.microseconds / 1000.0
18+
19+
if ms < ideal_ms:
20+
time.sleep((ideal_ms - ms) / 1000.0)
21+
22+
start_time_internal_freq_control = finish_time

‎exercises/conveyor_exercise/python_template/HAL.py‎

Lines changed: 361 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,361 @@
1+
print("HAL initializing", flush=True)
2+
##############################################################################
3+
# JdeROBOT ROBOTICS ACADEMY (http://jderobot.github.io/RoboticsAcademy/)
4+
# API PICK and PLACE exercise, including:
5+
# Robot Info: get_TCP_pose, get_Joint_states
6+
# Kinematics: MoveAbsJ, MoveJoint, MoveLinear, MoveSingleJ
7+
# MoveRelLinear, MoveRelReor
8+
# Gripper: GripperSet, attach, dettach
9+
#
10+
# VERSION: 1.0
11+
# DATE: April 21, 2025
12+
# AUTHOR: Diego Martin (diego.martin.martin@gmail.com)
13+
#
14+
# ======= Acknowledgments =======
15+
# IFRA-Cranfield nice "ROS2 Sim-to-Real Robot Control" package
16+
# URL: https://github.com/IFRA-Cranfield/ros2_SimRealRobotControl
17+
##############################################################################
18+
19+
import sys, os, time, math
20+
import rclpy
21+
22+
# import tf_transformations
23+
import numpy as np
24+
25+
from rclpy.node import Node
26+
from sensor_msgs.msg import JointState
27+
from ros2srrc_data.msg import Robpose
28+
from linkattacher_msgs.srv import AttachLink, DetachLink
29+
from ament_index_python.packages import get_package_share_directory
30+
31+
# Build PATH and import Python classes from IFRA package:
32+
PATH = os.path.join(get_package_share_directory("ros2srrc_execution"), "python")
33+
34+
PATH_ROB = PATH + "/robot"
35+
sys.path.append(PATH_ROB)
36+
from robot import RBT
37+
38+
PATH_EE = PATH + "/endeffector"
39+
sys.path.append(PATH_EE)
40+
from robotiq_ur import RobotiqGRIPPER
41+
42+
# Import ROS2 Custom Messages from IFRA package:
43+
from ros2srrc_data.msg import Action
44+
from ros2srrc_data.msg import Joint
45+
from ros2srrc_data.msg import Joints
46+
from ros2srrc_data.msg import Xyz
47+
from ros2srrc_data.msg import Ypr
48+
from ros2srrc_data.msg import Robpose
49+
50+
# Inicialization
51+
rclpy.init(args=None)
52+
UR5 = RBT()
53+
54+
55+
#################################### ROBOT KINEMATICS ###################################################
56+
# MoveAbsJ. Absolute Joints in degrees, speed max 1.0, wait time after movement in seconds
57+
def MoveAbsJ(absolute_joints, speed, wait_time):
58+
59+
ACTION = Action()
60+
ACTION.action = "MoveJ"
61+
ACTION.speed = float(speed)
62+
63+
INPUT = Joints()
64+
INPUT.joint1 = float(absolute_joints[0])
65+
INPUT.joint2 = float(absolute_joints[1])
66+
INPUT.joint3 = float(absolute_joints[2])
67+
INPUT.joint4 = float(absolute_joints[3])
68+
INPUT.joint5 = float(absolute_joints[4])
69+
INPUT.joint6 = float(absolute_joints[5])
70+
ACTION.movej = INPUT
71+
72+
EXECUTION = UR5.Move_EXECUTE(ACTION)
73+
74+
# Print movement results if movement succeeded
75+
if EXECUTION["Success"] == True:
76+
print(f"Robot moved to Joint Angular Goal: {absolute_joints}")
77+
print(
78+
f"Movement Execution Time: {EXECUTION['ExecTime']} s at Robot Speed: {speed*100} %"
79+
)
80+
else:
81+
print("Robot movement FAILED, check REASON in MoveIt output")
82+
83+
# Wait till next movement
84+
time.sleep(wait_time)
85+
print(f"Waiting {wait_time} s")
86+
print("")
87+
88+
89+
# MoveLinear. Linear movement to absolute pose XYZ with YPR absolute orientation in degrees
90+
# Speed max 1.0, wait time after movement in seconds
91+
def MoveLinear(abs_xyz, abs_ypr, speed, wait_time):
92+
93+
roll = math.radians(abs_ypr[0]) # Converts XYR to rad
94+
pitch = math.radians(abs_ypr[1])
95+
yaw = math.radians(abs_ypr[2])
96+
97+
# Quaternion from YPT in rad
98+
qx = np.sin(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) - np.cos(
99+
roll / 2
100+
) * np.sin(pitch / 2) * np.sin(yaw / 2)
101+
qy = np.cos(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2) + np.sin(
102+
roll / 2
103+
) * np.cos(pitch / 2) * np.sin(yaw / 2)
104+
qz = np.cos(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2) - np.sin(
105+
roll / 2
106+
) * np.sin(pitch / 2) * np.cos(yaw / 2)
107+
qw = np.cos(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) + np.sin(
108+
roll / 2
109+
) * np.sin(pitch / 2) * np.sin(yaw / 2)
110+
111+
InputPose = Robpose()
112+
InputPose.x = float(abs_xyz[0])
113+
InputPose.y = float(abs_xyz[1])
114+
InputPose.z = float(abs_xyz[2])
115+
InputPose.qx = qx
116+
InputPose.qy = qy
117+
InputPose.qz = qz
118+
InputPose.qw = qw
119+
120+
EXECUTION = UR5.RobMove_EXECUTE("LIN", float(speed), InputPose)
121+
122+
# Print movement results if movement succeeded
123+
if EXECUTION["Success"] == True:
124+
print(f"Robot moved linearly to Abs XYZ: {abs_xyz} and Abs YPR: {abs_ypr}")
125+
print(
126+
f"Movement Execution Time: {EXECUTION['ExecTime']} s at Robot Speed: {speed*100} %"
127+
)
128+
else:
129+
print("Robot movement FAILED, check REASON in MoveIt output")
130+
131+
# Wait till next movement
132+
time.sleep(wait_time)
133+
print(f"Waiting {wait_time} s")
134+
print("")
135+
136+
137+
# MoveJoint. Point-to-point movement to absolute pose XYZ with YPR absolute orientation in degrees
138+
# Speed max 1.0, wait time after movement in seconds
139+
def MoveJoint(abs_xyz, abs_ypr, speed, wait_time):
140+
141+
roll = math.radians(abs_ypr[0]) # Converts XYR to rad
142+
pitch = math.radians(abs_ypr[1])
143+
yaw = math.radians(abs_ypr[2])
144+
145+
# Quaternion from YPT in rad
146+
qx = np.sin(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) - np.cos(
147+
roll / 2
148+
) * np.sin(pitch / 2) * np.sin(yaw / 2)
149+
qy = np.cos(roll / 2) * np.sin(pitch / 2) * np.cos(yaw / 2) + np.sin(
150+
roll / 2
151+
) * np.cos(pitch / 2) * np.sin(yaw / 2)
152+
qz = np.cos(roll / 2) * np.cos(pitch / 2) * np.sin(yaw / 2) - np.sin(
153+
roll / 2
154+
) * np.sin(pitch / 2) * np.cos(yaw / 2)
155+
qw = np.cos(roll / 2) * np.cos(pitch / 2) * np.cos(yaw / 2) + np.sin(
156+
roll / 2
157+
) * np.sin(pitch / 2) * np.sin(yaw / 2)
158+
159+
InputPose = Robpose()
160+
InputPose.x = float(abs_xyz[0])
161+
InputPose.y = float(abs_xyz[1])
162+
InputPose.z = float(abs_xyz[2])
163+
InputPose.qx = qx
164+
InputPose.qy = qy
165+
InputPose.qz = qz
166+
InputPose.qw = qw
167+
168+
EXECUTION = UR5.RobMove_EXECUTE("PTP", float(speed), InputPose)
169+
170+
# Print movement results if movement succeeded
171+
if EXECUTION["Success"] == True:
172+
print(
173+
f"Robot moved Point-to-Point to Abs XYZ: {abs_xyz} and Abs YPR: {abs_ypr}"
174+
)
175+
print(
176+
f"Movement Execution Time: {EXECUTION['ExecTime']} s at Robot Speed: {speed*100} %"
177+
)
178+
else:
179+
print("Robot movement FAILED, check REASON in MoveIt output")
180+
181+
# Wait till next movement
182+
time.sleep(wait_time)
183+
print(f"Waiting {wait_time} s")
184+
print("")
185+
186+
187+
# MoveRelLinear. Linear movement, relative cartesian coordinates
188+
# Speed max 1.0, wait time after movement in seconds
189+
def MoveRelLinear(relative_xyz, speed, wait_time):
190+
ACTION = Action()
191+
ACTION.action = "MoveL"
192+
ACTION.speed = float(speed)
193+
194+
INPUT = Xyz()
195+
INPUT.x = float(relative_xyz[0])
196+
INPUT.y = float(relative_xyz[1])
197+
INPUT.z = float(relative_xyz[2])
198+
ACTION.movel = INPUT
199+
200+
EXECUTION = UR5.Move_EXECUTE(ACTION)
201+
202+
# Print movement results if movement succeeded
203+
if EXECUTION["Success"] == True:
204+
print(f"Robot moved LINEARLY by a relative increment of : {relative_xyz}")
205+
print(
206+
f"Movement Execution Time: {EXECUTION['ExecTime']} s at Robot Speed: {speed*100} %"
207+
)
208+
else:
209+
print("Robot movement FAILED, check REASON in MoveIt output")
210+
211+
212+
# MoveSingleJ. Relative angle in degrees, speed max 1.0, wait time after movement in seconds
213+
def MoveSingleJ(joint_number, relative_angle, speed, wait_time):
214+
ACTION = Action()
215+
ACTION.action = "MoveR"
216+
ACTION.speed = float(speed)
217+
218+
INPUT = Joint()
219+
INPUT.joint = str(joint_number)
220+
INPUT.value = float(relative_angle)
221+
ACTION.mover = INPUT
222+
223+
EXECUTION = UR5.Move_EXECUTE(ACTION)
224+
225+
# Print movement results if movement succeeded
226+
if EXECUTION["Success"] == True:
227+
print(f"Robot moved {joint_number} in {relative_angle} degrees")
228+
print(
229+
f"Movement Execution Time: {EXECUTION['ExecTime']} s at Robot Speed: {speed*100} %"
230+
)
231+
else:
232+
print("Robot movement FAILED, check REASON in MoveIt output")
233+
234+
# Wait till next movement
235+
time.sleep(wait_time)
236+
print(f"Waiting {wait_time} s")
237+
print("")
238+
239+
240+
# Relative Reorient given relative Euler Angles
241+
# Speed max 1.0, wait time after movement in seconds
242+
def MoveRelReor(relative_ypr, speed, wait_time):
243+
ACTION = Action()
244+
ACTION.action = "MoveROT"
245+
ACTION.speed = float(speed)
246+
247+
INPUT = Ypr()
248+
INPUT.pitch = float(relative_ypr[0])
249+
INPUT.yaw = float(relative_ypr[1])
250+
INPUT.roll = float(relative_ypr[2])
251+
ACTION.moverot = INPUT
252+
253+
EXECUTION = UR5.Move_EXECUTE(ACTION)
254+
255+
# Print movement results if movement succeeded
256+
if EXECUTION["Success"] == True:
257+
print(f"TCP reoriented by a relative increment of : {relative_ypr}")
258+
print(
259+
f"Movement Execution Time: {EXECUTION['ExecTime']} s at Robot Speed: {speed*100} %"
260+
)
261+
else:
262+
print("Robot movement FAILED, check REASON in MoveIt output")
263+
264+
# Wait till next movement
265+
time.sleep(wait_time)
266+
print(f"Waiting {wait_time} s")
267+
print("")
268+
269+
270+
###################################### GRIPPER ###################################################
271+
class LinkAttacherClient(Node):
272+
def __init__(self):
273+
super().__init__("link_attacher_client")
274+
self.attach_client = self.create_client(AttachLink, "/ATTACHLINK")
275+
self.detach_client = self.create_client(DetachLink, "/DETACHLINK")
276+
277+
while not self.attach_client.wait_for_service(timeout_sec=1.0):
278+
self.get_logger().info("Attach service not available, waiting again...")
279+
while not self.detach_client.wait_for_service(timeout_sec=1.0):
280+
self.get_logger().info("Detach service not available, waiting again...")
281+
282+
def send_attach_request(self, model1_name, link1_name, model2_name, link2_name):
283+
request = AttachLink.Request()
284+
request.model1_name = model1_name
285+
request.link1_name = link1_name
286+
request.model2_name = model2_name
287+
request.link2_name = link2_name
288+
289+
future = self.attach_client.call_async(request)
290+
rclpy.spin_until_future_complete(self, future)
291+
return future.result()
292+
293+
def send_detach_request(self, model1_name, link1_name, model2_name, link2_name):
294+
request = DetachLink.Request()
295+
request.model1_name = model1_name
296+
request.link1_name = link1_name
297+
request.model2_name = model2_name
298+
request.link2_name = link2_name
299+
300+
future = self.detach_client.call_async(request)
301+
rclpy.spin_until_future_complete(self, future)
302+
return future.result()
303+
304+
305+
# Attach object to gripper. Must be called explicitely
306+
def attach(item):
307+
link_attacher_client = LinkAttacherClient()
308+
309+
# Attach service call
310+
attach_response = link_attacher_client.send_attach_request(
311+
"ur5", "EE_robotiq_2f85", item, item
312+
)
313+
link_attacher_client.get_logger().info(
314+
"Attach Response: %s" % attach_response.success
315+
)
316+
317+
318+
# Dettach all objects. It is always called when gripper is set to full open (0%)
319+
def dettach():
320+
link_attacher_client = LinkAttacherClient()
321+
322+
# Detach operation for all possible objects when gripper is set to 0%
323+
link_attacher_client.send_detach_request(
324+
"ur5", "EE_robotiq_2f85", "red_box", "red_box"
325+
)
326+
link_attacher_client.send_detach_request(
327+
"ur5", "EE_robotiq_2f85", "yellow_box", "yellow_box"
328+
)
329+
link_attacher_client.send_detach_request(
330+
"ur5", "EE_robotiq_2f85", "blue_sphere", "blue_sphere"
331+
)
332+
link_attacher_client.send_detach_request(
333+
"ur5", "EE_robotiq_2f85", "green_cylinder", "green_cylinder"
334+
)
335+
336+
337+
# Gripper closing and opeining to a given percentage (100% full open, 0% full closed)
338+
# Speed max 1.0, wait time after movement in seconds
339+
def GripperSet(relative_closure, wait_time):
340+
ACTION = Action()
341+
ACTION.action = "MoveG"
342+
ACTION.speed = float(1) # Gripper speed not working for Robotiq 85, set to 100%
343+
344+
ACTION.moveg = float(relative_closure)
345+
346+
EXECUTION = UR5.Move_EXECUTE(ACTION)
347+
348+
# Print movement results if movement succeeded
349+
if EXECUTION["Success"] == True:
350+
print(f"Gripper set to a percentage of: {relative_closure} %")
351+
print(f"Movement Execution Time: {EXECUTION['ExecTime']} s")
352+
if relative_closure == 0:
353+
dettach() # Automatic object dettach from gripper when full open (0%)
354+
355+
else:
356+
print("Gripper closing FAILED, check REASON in MoveIt output")
357+
358+
# Wait till next movement
359+
time.sleep(wait_time)
360+
print(f"Waiting {wait_time} s")
361+
print("")

0 commit comments

Comments
 (0)