added the low-pass filter. needs testing tho. the only thing not tried yet is...

added the low-pass filter. needs testing tho. the only thing not tried yet is hybrid force-motion control, that is the next and final thing that could be good for the demo

python/convenience_tool_box/open_close_gripper.py

 from rtde_control import RTDEControlInterface as RTDEControl
 from rtde_receive import RTDEReceiveInterface as RTDEReceive
 from rtde_io import RTDEIOInterface
-from robotiq_gripper import RobotiqGripper
+from ur_simple_control.util.robotiq_gripper import RobotiqGripper
 import pinocchio as pin
 import numpy as np
 import os
@@ -12,7 +12,7 @@ import matplotlib.pyplot as plt
 
 def handler(signum, frame):
     gripper.move(255,255,255)
-    time.sleep(3)
+    time.sleep(2)
     exit()
 
 signal.signal(signal.SIGINT, handler)
@@ -22,4 +22,6 @@ gripper.connect("192.168.1.102", 63352)
 gripper.activate()
 time.sleep(3)
 gripper.move(0,100,100)
-time.sleep(300)
+#time.sleep(300)
+input("press Enter to close and quit")
+handler(None, None)

python/examples/drawing_from_input_drawing.py

@@ -188,7 +188,11 @@ def controlLoopWriting(dmp, tc, controller, robot, i, past_data):
     # rolling average
     # TODO: try doing a low-pass filter instead
     # TODO: check where the shape of this is correct (should be, but check)
-    wrench = np.average(np.array(past_data['wrench']), axis=0)
+    #wrench = np.average(np.array(past_data['wrench']), axis=0)
+    # first-order low pass filtering instead
+    # beta is a smoothing coefficient, smaller values smooth more, has to be in [0,1]
+    beta = 0.01
+    wrench = beta * wrench + (1 - beta) * past_data['wrench'][-1]
     pin.forwardKinematics(robot.model, robot.data, q)
     J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
     dq = robot.getQd()[:6].reshape((6,1))
@@ -263,6 +267,17 @@ if __name__ == "__main__":
         # TODO: make this an argument once the rest is OK
         # ---> just go to the board while you have the marker ready to find this
         # ---> do that right here
+        # put this in a correct place
+        #old_speed_slider = robot.speed_slider
+        #robot.setSpeedSlider(0.3)
+        #speed = [0, 0, -0.1, 0, 0, 0]
+        #robot.rtde_control.moveUntilContact(speed)
+        #current_pose = np.array(robot.rtde_receive.getActualTCPPose())
+        ## i only care about the z because i'm fixing the path atm
+        #marker_offset = translation_vector[2] - current_pose[2]
+        #robot.setSpeedSlider(old_speed_slider)
+        
+        # TODO: change magic -0.0938 with market_offset (you first need to check that it works)
         path = path + np.array([0.0, 0.0, -0.0938])
         # create a joint space trajectory based on the 3D path
         joint_trajectory = clikCartesianPathIntoJointPath(path, args, robot, \

python/examples/path_in_pixels.csv

-0.10362,0.20547
-0.10511,0.20547
-0.10511,0.20697
-0.10808,0.20697
-0.10957,0.20697
-0.12297,0.20997
-0.12743,0.20997
-0.12892,0.20997
-0.14529,0.20997
-0.14827,0.20997
-0.15273,0.20997
-0.16910,0.21447
-0.17357,0.21447
-0.17803,0.21447
-0.19143,0.21447
-0.19589,0.21447
-0.19887,0.21447
-0.20929,0.21447
-0.21375,0.21447
-0.21673,0.21447
-0.22119,0.21597
-0.23161,0.21597
-0.23608,0.21597
-0.24054,0.21597
-0.26286,0.22048
-0.27328,0.22048
-0.31644,0.22799
-0.33132,0.22799
-0.34025,0.22799
-0.34621,0.22949
-0.37002,0.22949
-0.37448,0.22949
-0.39085,0.23399
-0.39234,0.23399
-0.39532,0.23399
-0.39978,0.23399
-0.40127,0.23549
-0.41467,0.23549
-0.41616,0.23549
-0.42508,0.23850
-0.42657,0.23850
-0.42657,0.24000
-0.42508,0.24300
-0.41913,0.24600
-0.40127,0.26102
-0.39681,0.26552
-0.38490,0.27303
-0.35811,0.29555
-0.35216,0.30155
-0.34770,0.30756
-0.32686,0.32858
-0.32388,0.33158
-0.32091,0.33458
-0.31495,0.33909
-0.30156,0.35861
-0.26882,0.39764
-0.26584,0.40215
-0.25840,0.41416
-0.25691,0.41566
-0.25542,0.42016
-0.25245,0.42467
-0.25245,0.42617
-0.24947,0.43668
-0.24947,0.43818
-0.24947,0.44869
-0.24947,0.45470
-0.26584,0.48172
-0.27179,0.48773
-0.27775,0.49373
-0.30007,0.52226
-0.30602,0.52826
-0.31495,0.53127
-0.32091,0.53877
-0.32984,0.54328
-0.34918,0.56280
-0.35365,0.56580
-0.36555,0.57331
-0.38639,0.58231
-0.38937,0.58231
-0.39234,0.58231
-0.40276,0.58532
-0.40723,0.58682
-0.42508,0.59132
-0.42657,0.59282
-0.43104,0.59282
-0.43401,0.59282
-0.44443,0.59282
-0.44890,0.59583
-0.45187,0.59583
-0.46080,0.59583
-0.46527,0.59583
-0.47271,0.59583
-0.47866,0.59583
-0.48759,0.59583
-0.51885,0.58982
-0.52926,0.58682
-0.53522,0.58532
-0.54563,0.58532
-0.58879,0.57331
-0.59772,0.57030
-0.60219,0.57030
-0.62302,0.56280
-0.62749,0.56280
-0.63195,0.55979
-0.64535,0.55679
-0.64684,0.55679
-0.65130,0.55379
-0.66172,0.55078
-0.66321,0.55078
-0.66618,0.54928
-0.66916,0.54628
-0.67362,0.54478
-0.67511,0.54328
-0.67660,0.54178
-0.68255,0.53577
-0.68255,0.53427
-0.68404,0.52977
-0.68404,0.52826
-0.68702,0.51926
-0.68851,0.51775
-0.68851,0.51475
-0.69297,0.50424
-0.69446,0.50274
-0.69446,0.49824
-0.70339,0.47572
-0.70637,0.46521
-0.70934,0.45620
-0.71530,0.43968
-0.71678,0.43368
-0.71678,0.43217
-0.72125,0.41716
-0.72125,0.41266
-0.72125,0.40815
-0.72571,0.37662
-0.72571,0.37212
-0.72571,0.36912
-0.72571,0.36461
-0.72571,0.35410
-0.72571,0.35110
-0.72571,0.33458
-0.72571,0.33308
-0.72571,0.33008
-0.72571,0.32107
-0.71678,0.29104
-0.71530,0.28654
-0.71232,0.27753
-0.70190,0.25201
-0.69892,0.24150
-0.69744,0.23699
-0.68851,0.22648
-0.68107,0.20997
-0.67660,0.20246
-0.67362,0.19796
-0.65725,0.17093
-0.65577,0.16493
-0.64237,0.14691
-0.63939,0.14541
-0.63791,0.14091
-0.63642,0.13940
-0.63493,0.13790
-0.63493,0.13640
-0.63344,0.13640
-0.63344,0.13490
-0.63791,0.13490
-0.63939,0.13790
-0.64832,0.13790
-0.66172,0.13940
-0.66767,0.14091
-0.68107,0.14541
-0.68553,0.14541
-0.69000,0.14541
-0.69148,0.14541
-0.70785,0.14841
-0.71232,0.14841
-0.71530,0.15142
-0.71976,0.15142
-0.73018,0.15142
-0.73464,0.15442
-0.73911,0.15442
-0.74060,0.15442
-0.75548,0.15742
-0.75994,0.15742
-0.76143,0.15742
-0.76590,0.16042
-0.77483,0.16042
-0.77631,0.16042
-0.77780,0.16042
-0.77929,0.16192
-0.78078,0.16192
-0.78227,0.16192
-0.78376,0.16192
-0.78524,0.16192
-0.78673,0.16192
-0.78822,0.16192
-0.78971,0.16192
-0.79120,0.16192
-0.79120,0.16192
+0.40276,0.80752
+0.41318,0.81052
+0.45336,0.82404
+0.51140,0.83305
+0.52777,0.83605
+0.53522,0.83605
+0.57540,0.83605
+0.58879,0.83605
+0.59772,0.83605
+0.63493,0.83154
+0.64535,0.83154
+0.65874,0.82854
+0.69297,0.81653
+0.70190,0.81353
+0.70637,0.81203
+0.73613,0.80152
+0.74208,0.79551
+0.74804,0.78951
+0.77334,0.77449
+0.77780,0.76849
+0.78376,0.76248
+0.79566,0.75497
+0.80757,0.74597
+0.80906,0.74146
+0.81650,0.73546
+0.83733,0.71444
+0.85222,0.68891
+0.85817,0.68291
+0.86115,0.67690
+0.86710,0.66639
+0.87007,0.65888
+0.88049,0.63336
+0.88198,0.63186
+0.88198,0.62285
+0.88347,0.62135
+0.88347,0.61985
+0.88347,0.61835
+0.88347,0.61685
+0.88347,0.61534
+0.88198,0.61384
+0.88049,0.61234
+0.86710,0.59883
+0.86115,0.59432
+0.85668,0.59132
+0.83882,0.57481
+0.83287,0.56880
+0.82543,0.56580
+0.80608,0.55078
+0.79566,0.54328
+0.77631,0.52977
+0.76738,0.52676
+0.76143,0.52076
+0.75101,0.51775
+0.74357,0.51475
+0.72125,0.50274
+0.71678,0.49974
+0.70785,0.49673
+0.68404,0.48172
+0.67362,0.47872
+0.67214,0.47722
+0.66172,0.47421
+0.65725,0.47121
+0.64832,0.47121
+0.64684,0.46971
+0.64237,0.46971
+0.64088,0.46971
+0.63642,0.46971
+0.63493,0.46971
+0.61856,0.46971
+0.61707,0.46971
+0.61261,0.46971
+0.60814,0.47271
+0.59475,0.47271
+0.58731,0.47421
+0.58284,0.47421
+0.55159,0.48022
+0.54415,0.48022
+0.51140,0.48472
+0.50099,0.48923
+0.49950,0.48923
+0.48313,0.49373
+0.48164,0.49523
+0.47717,0.49523
+0.47420,0.49673
+0.46824,0.49824
+0.46676,0.50124
+0.46527,0.50124
+0.45634,0.50724
+0.45336,0.50875
+0.45336,0.51025
+0.45187,0.51175
+0.44741,0.51775
+0.44592,0.51926
+0.44443,0.52076
+0.44146,0.52526
+0.43848,0.53127
+0.43699,0.53277
+0.43550,0.53727
+0.43253,0.54178
+0.43104,0.54328
+0.42955,0.54478
+0.42211,0.55529
+0.42062,0.55829
+0.41318,0.56880
+0.41020,0.57331
+0.40871,0.57631
+0.40574,0.58382
+0.40425,0.58682
+0.40276,0.58682
+0.40276,0.58832
+0.39532,0.60033
+0.39532,0.60183
+0.39234,0.60634
+0.38937,0.61384
+0.38937,0.61534
+0.38788,0.61985
+0.38788,0.62886
+0.38639,0.63036
+0.38639,0.63486
+0.38341,0.64387
+0.38341,0.64537
+0.38341,0.64687
+0.38193,0.64837
+0.38193,0.65288
+0.38193,0.65888
+0.37895,0.66789
+0.37895,0.66939
+0.37895,0.67240
+0.37895,0.68291
+0.37895,0.68741
+0.37597,0.69192
+0.37597,0.69642
+0.37597,0.70393
+0.37597,0.70843
+0.37597,0.70993
+0.37597,0.72044
+0.37597,0.72344
+0.37597,0.72495
+0.37597,0.73546
+0.37597,0.73996
+0.37597,0.74146
+0.38044,0.75798
+0.38044,0.76698
+0.38044,0.77149
+0.38193,0.77299
+0.38193,0.77749
+0.38193,0.77900
+0.38490,0.78951
+0.38490,0.79101
+0.38788,0.79551
+0.38937,0.80002
+0.38937,0.80302
+0.38937,0.80452
+0.39234,0.80902
+0.39383,0.81353
+0.39383,0.81503
+0.39830,0.82404
+0.39978,0.83004
+0.39978,0.83154
+0.40574,0.83605
+0.40723,0.83755
+0.40871,0.83905
+0.41020,0.83905
+0.41169,0.84205
+0.41318,0.84205
+0.41467,0.84205
+0.41616,0.84205
+0.41764,0.84205
+0.41764,0.84205