update

Author: yuth

README.md

@@ -1 +1,5 @@
-# Labbing Python
+# Lab Python
+This Package is create to collect some of my python code that I have learnt. [For Mom!]
+
+### Reference [List](./doc/)
+There are detail of the package, read more in the references list link above.

algorithms/insertion_sort.py

@@ -1,33 +1,34 @@
 def insertion_sort_increase(A):
-    for j in range(1,len(A)):
+    for j in range(1, len(A)):
         key = A[j]
         # insert A[j] into the sorted seq A[1 ... j-1]
-        i = j-1
-        while i >= 0 and A[i]>key: # important for increase ( if i+1 is smaller than i = swap it)
-            A[i+1] = A[i]
+        i = j - 1
+        while i >= 0 and A[i] > key:  # important for increase ( if i+1 is smaller than i = swap it)
+            A[i + 1] = A[i]
             i = i - 1
-        A[i+1] = key
+        A[i + 1] = key
 
     return A
 
+
 def insertion_sort_deincrease(A):
-    for j in range(1,len(A)):
+    for j in range(1, len(A)):
         key = A[j]
         # insert A[j] into the sorted seq A[1 ... j-1]
-        i = j-1
-        while i >= 0 and A[i]<key: # important for increase ( if i+1 is bigger than i = swap it)
-            A[i+1] = A[i]
+        i = j - 1
+        while i >= 0 and A[i] < key:  # important for increase ( if i+1 is bigger than i = swap it)
+            A[i + 1] = A[i]
             i = i - 1
-        A[i+1] = key
+        A[i + 1] = key
 
     return A
 
 
-A = [5,2,4,6,1,3]
-A = [31,41,59,26,41,58]
+A = [5, 2, 4, 6, 1, 3]
+A = [31, 41, 59, 26, 41, 58]
 print(A)
 result_increase = insertion_sort_increase(A)
 print(result_increase)
 
 result_deincrease = insertion_sort_deincrease(A)
-print(result_deincrease)
+print(result_deincrease)

algorithms/linear_search.py

@@ -1,14 +1,14 @@
-def linear_search(A,v):
+def linear_search(A, v):
     for j in range(len(A)):
         if A[j] == v:
             print("Found the number at: index")
 
-    return j # index of found number
+    return j  # index of found number
 
 
-A = [5,2,4,6,1,3]
+A = [5, 2, 4, 6, 1, 3]
 
 v = 6
 print(A)
-result = linear_search(A,v)
-print(result)
+result = linear_search(A, v)
+print(result)

doc/cover.png

@@ -0,0 +1,27 @@
+from rtde_control import RTDEControlInterface as RTDEControl
+
+rtde_c = RTDEControl("127.0.0.1")
+
+task_frame = [0, 0, 0, 0, 0, 0]
+selection_vector = [0, 0, 1, 0, 0, 0]
+wrench_down = [0, 0, -10, 0, 0, 0]
+wrench_up = [0, 0, 10, 0, 0, 0]
+force_type = 2
+limits = [2, 2, 1.5, 1, 1, 1]
+joint_q = [-1.54, -1.83, -2.28, -0.59, 1.60, 0.023]
+
+# Move to initial joint position with a regular moveJ
+rtde_c.moveJ(joint_q)
+
+# Execute 500Hz control loop for 4 seconds, each cycle is 2ms
+for i in range(2000):
+    t_start = rtde_c.initPeriod()
+    # First move the robot down for 2 seconds, then up for 2 seconds
+    if i > 1000:
+        rtde_c.forceMode(task_frame, selection_vector, wrench_up, force_type, limits)
+    else:
+        rtde_c.forceMode(task_frame, selection_vector, wrench_down, force_type, limits)
+    rtde_c.waitPeriod(t_start)
+
+rtde_c.forceModeStop()
+rtde_c.stopScript()

hardware/urrtde/forcemode_example.py

@@ -0,0 +1,37 @@
+from rtde_io import RTDEIOInterface as RTDEIO
+from rtde_receive import RTDEReceiveInterface as RTDEReceive
+import time
+
+rtde_io_ = RTDEIO("127.0.0.1")
+rtde_receive_ = RTDEReceive("127.0.0.1")
+
+# How-to set and get standard and tool digital outputs. Notice that we need the
+# RTDEIOInterface for setting an output and RTDEReceiveInterface for getting the state
+# of an output.
+
+if rtde_receive_.getDigitalOutState(7):
+    print("Standard digital out (7) is HIGH")
+else:
+    print("Standard digital out (7) is LOW")
+
+if rtde_receive_.getDigitalOutState(16):
+    print("Tool digital out (16) is HIGH")
+else:
+    print("Tool digital out (16) is LOW")
+
+rtde_io_.setStandardDigitalOut(7, True)
+rtde_io_.setToolDigitalOut(0, True)
+time.sleep(0.01)
+
+if rtde_receive_.getDigitalOutState(7):
+    print("Standard digital out (7) is HIGH")
+else:
+    print("Standard digital out (7) is LOW")
+
+if rtde_receive_.getDigitalOutState(16):
+    print("Tool digital out (16) is HIGH")
+else:
+    print("Tool digital out (16) is LOW")
+
+# How to set a analog output with a specified current ratio
+rtde_io_.setAnalogOutputCurrent(1, 0.25)

hardware/urrtde/io_example.py

@@ -0,0 +1,19 @@
+import numpy as np
+import rtde_control
+import rtde_receive
+
+
+rtde_c = rtde_control.RTDEControlInterface("192.168.0.3")
+rtde_r = rtde_receive.RTDEReceiveInterface("192.168.0.3")
+
+# from techpendant
+# -10.0 -80.0 -97.0 -30.0 9.0 0.0
+
+# it seem ros joint is not start in order
+# elbow_joint, shoulder_lift_joint, shoulder_pan_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint
+rosjointvalue = np.rad2deg([-1.692978858947754, -1.3962847751430054, -0.17454559007753545, -0.5235684675029297, 0.15709757804870605, -3.987947572881012e-05])
+
+rtdejointvalue = np.rad2deg(rtde_r.getActualQ())
+
+# > rosjointvalue: [-9.70005434e+01 -8.00012246e+01 -1.00007256e+01 -2.99982635e+01 9.00102819e+00 -2.28492565e-03]
+# > rtdejointvalue: [-9.99796625e+00 -8.00012246e+01 -9.70033028e+01 -3.00010229e+01 9.00034518e+00 -8.91567058e-04]

hardware/urrtde/joint_ordered.py

@@ -0,0 +1,37 @@
+from rtde_control import RTDEControlInterface as RTDEControl
+from rtde_receive import RTDEReceiveInterface as RTDEReceive
+import time
+
+rtde_c = RTDEControl("127.0.0.1")
+rtde_r = RTDEReceive("127.0.0.1")
+init_q = rtde_r.getActualQ()
+
+# Target in the robot base
+new_q = init_q[:]
+new_q[0] += 0.20
+
+# Move asynchronously in joint space to new_q, we specify asynchronous behavior by setting the async parameter to
+# 'True'. Try to set the async parameter to 'False' to observe a default synchronous movement, which cannot be stopped
+# by the stopJ function due to the blocking behaviour.
+rtde_c.moveJ(new_q, 1.05, 1.4, True)
+time.sleep(0.2)
+# Stop the movement before it reaches new_q
+rtde_c.stopJ(0.5)
+
+# Target in the Z-Axis of the TCP
+target = rtde_r.getActualTCPPose()
+target[2] += 0.10
+
+# Move asynchronously in cartesian space to target, we specify asynchronous behavior by setting the async parameter to
+# 'True'. Try to set the async parameter to 'False' to observe a default synchronous movement, which cannot be stopped
+# by the stopL function due to the blocking behaviour.
+rtde_c.moveL(target, 0.25, 0.5, True)
+time.sleep(0.2)
+# Stop the movement before it reaches target
+rtde_c.stopL(0.5)
+
+# Move back to initial joint configuration
+rtde_c.moveJ(init_q)
+
+# Stop the RTDE control script
+rtde_c.stopScript()

hardware/urrtde/move_async_example.py

@@ -0,0 +1,42 @@
+from rtde_control import RTDEControlInterface as RTDEControl
+from rtde_control import Path, PathEntry
+import time
+
+rtde_c = RTDEControl("localhost")
+
+path = Path()
+vel = 0.5
+acc = 4.0
+blend = 0.099
+path.addEntry(PathEntry(PathEntry.MoveJ, PathEntry.PositionTcpPose, [-0.140, -0.400, 0.100, 0, 3.14, 0, vel, acc, 0.0]))
+path.addEntry(PathEntry(PathEntry.MoveL, PathEntry.PositionTcpPose, [-0.140, -0.400, 0.300, 0, 3.14, 0, vel, acc, blend]))
+path.addEntry(PathEntry(PathEntry.MoveL, PathEntry.PositionTcpPose, [-0.140, -0.600, 0.300, 0, 3.14, 0, vel, acc, blend]))
+path.addEntry(PathEntry(PathEntry.MoveJ, PathEntry.PositionTcpPose, [-0.140, -0.600, 0.100, 0, 3.14, 0, vel, acc, blend]))
+path.addEntry(PathEntry(PathEntry.MoveJ, PathEntry.PositionTcpPose, [-0.140, -0.400, 0.100, 0, 3.14, 0, vel, acc, 0.0]))
+
+# First move given path synchronously
+print("Move path synchronously...")
+rtde_c.movePath(path, False)
+print("Path finished!")
+
+# Now move given path asynchronously
+print("Move path asynchronously with progress feedback...")
+rtde_c.movePath(path, True)
+
+# Wait for start of asynchronous operation
+while not rtde_c.getAsyncOperationProgressEx().isAsyncOperationRunning():
+    time.sleep(0.010)
+print("Async path started.. ")
+
+# Wait for end of asynchronous operation
+waypoint = -1
+while rtde_c.getAsyncOperationProgressEx().isAsyncOperationRunning():
+    time.sleep(0.2)
+    new_waypoint = rtde_c.getAsyncOperationProgress()
+    if new_waypoint != waypoint:
+        waypoint = new_waypoint
+        print("Moving to path waypoint ")
+
+print("Async path finished...\n\n")
+rtde_c.stopScript()
+

hardware/urrtde/move_path_async_example.py

@@ -0,0 +1,7 @@
+from rtde_control import RTDEControlInterface as RTDEControl
+
+rtde_c = RTDEControl("127.0.0.1")
+speed = [0, 0, -0.100, 0, 0, 0]
+rtde_c.moveUntilContact(speed)
+
+rtde_c.stopScript()

hardware/urrtde/move_until_contact.py

@@ -0,0 +1,17 @@
+from rtde_control import RTDEControlInterface as RTDEControl
+
+rtde_c = RTDEControl("127.0.0.1")
+
+velocity = 0.5
+acceleration = 0.5
+blend_1 = 0.0
+blend_2 = 0.02
+blend_3 = 0.0
+path_pose1 = [-0.143, -0.435, 0.20, -0.001, 3.12, 0.04, velocity, acceleration, blend_1]
+path_pose2 = [-0.143, -0.51, 0.21, -0.001, 3.12, 0.04, velocity, acceleration, blend_2]
+path_pose3 = [-0.32, -0.61, 0.31, -0.001, 3.12, 0.04, velocity, acceleration, blend_3]
+path = [path_pose1, path_pose2, path_pose3]
+
+# Send a linear path with blending in between - (currently uses separate script)
+rtde_c.moveL(path)
+rtde_c.stopScript()

hardware/urrtde/movel_path_with_blend_example.py

@@ -0,0 +1,69 @@
+from rtde_control import RTDEControlInterface as RTDEControl
+from rtde_receive import RTDEReceiveInterface as RTDEReceive
+import datetime
+import math
+import os
+import psutil
+import sys
+
+
+def getCircleTarget(pose, timestep, radius=0.075, freq=1.0):
+    circ_target = pose[:]
+    circ_target[0] = pose[0] + radius * math.cos((2 * math.pi * freq * timestep))
+    circ_target[1] = pose[1] + radius * math.sin((2 * math.pi * freq * timestep))
+    return circ_target
+
+
+# Parameters
+vel = 0.05
+acc = 0.05
+rtde_frequency = 500.0
+dt = 1.0/rtde_frequency  # 2ms
+flags = RTDEControl.FLAG_VERBOSE | RTDEControl.FLAG_UPLOAD_SCRIPT
+ur_cap_port = 50002
+robot_ip = "192.168.0.3"
+
+lookahead_time = 0.1
+gain = 600
+
+# ur_rtde realtime priorities
+rt_receive_priority = 90
+rt_control_priority = 85
+
+rtde_r = RTDEReceive(robot_ip, rtde_frequency, [], True, False, rt_receive_priority)
+rtde_c = RTDEControl(robot_ip, rtde_frequency, flags, ur_cap_port, rt_control_priority)
+
+# Set application real-time priority
+os_used = sys.platform
+process = psutil.Process(os.getpid())
+if os_used == "win32":  # Windows (either 32-bit or 64-bit)
+    process.nice(psutil.REALTIME_PRIORITY_CLASS)
+elif os_used == "linux":  # linux
+    rt_app_priority = 80
+    param = os.sched_param(rt_app_priority)
+    try:
+        os.sched_setscheduler(0, os.SCHED_FIFO, param)
+    except OSError:
+        print("Failed to set real-time process scheduler to %u, priority %u" % (os.SCHED_FIFO, rt_app_priority))
+    else:
+        print("Process real-time priority set to: %u" % rt_app_priority)
+
+time_counter = 0.0
+
+# Move to init position using moveL
+actual_tcp_pose = rtde_r.getActualTCPPose()
+init_pose = getCircleTarget(actual_tcp_pose, time_counter)
+rtde_c.moveL(init_pose, vel, acc)
+
+try:
+    while True:
+        t_start = rtde_c.initPeriod()
+        servo_target = getCircleTarget(actual_tcp_pose, time_counter)
+        rtde_c.servoL(servo_target, vel, acc, dt, lookahead_time, gain)
+        rtde_c.waitPeriod(t_start)
+        time_counter += dt
+
+except KeyboardInterrupt:
+    print("Control Interrupted!")
+    rtde_c.servoStop()
+    rtde_c.stopScript()