diff --git a/.gitignore b/.gitignore
index af1c19092c6423e9143b1efb01c16bd7c21eeb7d..9f12f4e633167f31196961d94b18dfff0896ff3f 100644
--- a/.gitignore
+++ b/.gitignore
@@ -5,4 +5,4 @@ build/
**__pycache__
.vscode/
**.pickle
-*.csv
+**.csv
diff --git a/Dockerfile b/Dockerfile
index 485f285f41fd1f255906d3b779260167cf8b7b7d..a3af1230526027e6723f294562814f2a71771bea 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -76,7 +76,7 @@ RUN pip install -e ./SimpleManipulatorControl/python/
RUN conda config --add channels conda-forge
#RUN conda install --solver=classic conda-forge::conda-libmamba-solver conda-forge::libmamba conda-forge::libmambapy conda-forge::libarchive
RUN conda install -y casadi opencv
-RUN conda install --solver=classic -y pinocchio -c conda-forge
+RUN conda install --solver=classic -y pinocchio crocoddyl -c conda-forge
RUN pip install matplotlib meshcat ur_rtde argcomplete \
qpsolvers ecos example_robot_data meshcat_shapes \
pyqt6
diff --git a/TODOS b/TODOS
index 2b541ce555222c6d85e2c8f383c171388d8918fc..419550ac1f57d3785d59ba8ddcff3e919188813b 100644
--- a/TODOS
+++ b/TODOS
@@ -1,3 +1,6 @@
+START THE REAL-TIME PLOT CAUSES A DELAY BECAUSE YOU WAIT ON IT
+LET IT START ITSELF, DON'T GIVE THE SUCCESS FLAG BACK!!!!!!!!!
+
goal 1: starting points for student projects
---------------------------------------------
1. PUSHING GROUP: some multiprocessing to get camera outputs
@@ -152,6 +155,8 @@ goal 4: more controllers
goal 5: more convenience - set gripper payload etc automatically
------------------------
+1. test whether meshcat.display(q) is a blocking or a non-blocking call.
+ if it's a non-blocking call then you don't need to run a separate process to command the visualization
# TODO and NOTE the weight, TCP and inertial matrix needs to be set on the robot
# you already found an API in rtde_control for this, just put it in initialization
# under using/not-using gripper parameters
diff --git a/extensions_for_fun.md b/extensions_for_fun.md
new file mode 100644
index 0000000000000000000000000000000000000000..dd27d2c5ec8b350721c46fe06b94fff22899789a
--- /dev/null
+++ b/extensions_for_fun.md
@@ -0,0 +1,16 @@
+1. easy to use DMPs
+--------------------
+
+1.1. cartesian space dmp.
+----------------------------
+the current one unnecessarily assumes joint space,
+but if you rewrite the code it could do either based on an argument.
+as far as the dmp is concerned only the dimensions are different,
+and you can just ik the cartesian reference into joint space
+
+1.2. using multiple trajectories for a fit
+---------------------------------------------
+the fitting is already there, it just assumes a single trajectory (indirectly,
+the math is the same either way).
+adding more should literally just amount to verifying the dimensions
+in the math.
diff --git a/python/examples/camera_no_lag.py b/python/examples/camera_no_lag.py
index 122f08694037d7d4bb541a42305be3969954b630..9b84d9a187ca91a7e0c9b9e9b09518b8f24f5cd8 100644
--- a/python/examples/camera_no_lag.py
+++ b/python/examples/camera_no_lag.py
@@ -1,22 +1,75 @@
-import cv2
-
-#camera = cv2.VideoCapture(0)
-# if you have ip_webcam set-up you can do this (but you have put in the correct ip)
-camera = cv2.VideoCapture("http://192.168.219.153:8080/video")
-
-while True:
- (grabbed, frame) = camera.read()
-# frame = cv2.flip(frame, 1)
- scale_percent = 100 # percent of original size
- width = int(frame.shape[1] * scale_percent / 100)
- height = int(frame.shape[0] * scale_percent / 100)
- dim = (width, height)
-
- # resize image
- frame = cv2.resize(frame, dim, interpolation = cv2.INTER_AREA)
-# frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
- cv2.imshow("camera", frame)
- keypress = cv2.waitKey(1) & 0xFF
-
- if keypress == ord("q"):
- break
+from ur_simple_control.vision.vision import processCamera
+from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager, ProcessManager
+import argcomplete, argparse
+import numpy as np
+import time
+import pinocchio as pin
+from functools import partial
+
+
+def get_args():
+ parser = getMinimalArgParser()
+ parser.description = 'dummy camera output, but being processed \
+ in a different process'
+ argcomplete.autocomplete(parser)
+ args = parser.parse_args()
+ return args
+
+def controlLoopWithCamera(camera_manager : ProcessManager, args, robot : RobotManager, i, past_data):
+ """
+ controlLoopWithCamera
+ -----------------------------
+ do nothing while getting dummy camera input
+ """
+ breakFlag = False
+ log_item = {}
+ save_past_dict = {}
+ q = robot.getQ()
+
+ camera_output = camera_manager.getData()
+ #print(camera_output)
+
+ qd_cmd = np.zeros(robot.model.nv)
+
+ robot.sendQd(qd_cmd)
+
+ log_item['qs'] = q.reshape((robot.model.nq,))
+ log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+ log_item['camera_output'] = np.array([camera_output['x'],
+ camera_output['y']])
+ return breakFlag, save_past_dict, log_item
+
+if __name__ == "__main__":
+ args = get_args()
+ robot = RobotManager(args)
+
+ # cv2 camera device 0
+ device = 0
+ side_function = partial(processCamera, device)
+ init_value = {"x" : np.random.randint(0, 10),
+ "y" : np.random.randint(0, 10)}
+ camera_manager = ProcessManager(args, side_function, {}, 1, init_value=init_value)
+
+
+ log_item = {}
+ log_item['qs'] = np.zeros((robot.model.nq,))
+ log_item['dqs'] = np.zeros((robot.model.nv,))
+ log_item['camera_output'] = np.zeros(2)
+ controlLoop = partial(controlLoopWithCamera, camera_manager, args, robot)
+ loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
+ loop_manager.run()
+
+
+
+ # get expected behaviour here (library can't know what the end is - you have to do this here)
+ if not args.pinocchio_only:
+ robot.stopRobot()
+
+ if args.save_log:
+ robot.log_manager.plotAllControlLoops()
+
+ if args.visualize_manipulator:
+ robot.killManipulatorVisualizer()
+
+ if args.save_log:
+ robot.log_manager.saveLog()
diff --git a/python/examples/casadi_ocp_collision_avoidance.py b/python/examples/casadi_ocp_collision_avoidance.py
index dd2ef0bcca65630320d0124a45192c4ede780f7f..d2a3c105b50b7f3f5cd761f718979053296f03f2 100644
--- a/python/examples/casadi_ocp_collision_avoidance.py
+++ b/python/examples/casadi_ocp_collision_avoidance.py
@@ -5,10 +5,14 @@ import time
import argparse
from functools import partial
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
+from ur_simple_control.optimal_control.create_pinocchio_casadi_ocp import createCasadiIKObstacleAvoidanceOCP
from ur_simple_control.optimal_control.get_ocp_args import get_OCP_args
+from ur_simple_control.basics.basics import followKinematicJointTrajP
+import argcomplete
def get_args():
parser = getMinimalArgParser()
+ parser = get_OCP_args(parser)
parser.description = 'optimal control in pinocchio.casadi for obstacle avoidance'
# add more arguments here from different Simple Manipulator Control modules
argcomplete.autocomplete(parser)
@@ -18,7 +22,15 @@ def get_args():
if __name__ == "__main__":
args = get_args()
robot = RobotManager(args)
-
+ #T_goal = robot.defineGoalPointCLI()
+ #T_goal = pin.SE3.Random()
+ T_goal = robot.defineGoalPointCLI()
+ T_goal.rotation = robot.getT_w_e().rotation
+ if args.visualize_manipulator:
+ robot.manipulator_visualizer_queue.put(
+ {"Mgoal" : T_goal})
+ reference, opti = createCasadiIKObstacleAvoidanceOCP(args, robot, T_goal)
+ followKinematicJointTrajP(args, robot, reference)
# get expected behaviour here (library can't know what the end is - you have to do this here)
@@ -33,4 +45,3 @@ if __name__ == "__main__":
if args.save_log:
robot.log_manager.saveLog()
- #loop_manager.stopHandler(None, None)
diff --git a/python/examples/clik.py b/python/examples/clik.py
index 0ccf6c8015ed562cef0779e1187d5bea399af2d0..cd07a47a12b4aceccabd3e4c071cb4b41f90fe5c 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik.py
@@ -1,6 +1,7 @@
# PYTHON_ARGCOMPLETE_OK
import numpy as np
import time
+import pinocchio as pin
import argcomplete, argparse
from functools import partial
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
@@ -19,6 +20,7 @@ def get_args():
if __name__ == "__main__":
args = get_args()
robot = RobotManager(args)
+ print(robot.getT_w_e())
Mgoal = robot.defineGoalPointCLI()
compliantMoveL(args, robot, Mgoal)
robot.closeGripper()
diff --git a/python/examples/crocoddyl_mpc.py b/python/examples/crocoddyl_mpc.py
index 556f71ad5e158dea20bb854ae5c95ccc7274ae46..8b95290a283076097f374d8fcfd38271f0030835 100644
--- a/python/examples/crocoddyl_mpc.py
+++ b/python/examples/crocoddyl_mpc.py
@@ -14,6 +14,7 @@ from ur_simple_control.clik.clik import getClikArgs
import pinocchio as pin
import crocoddyl
import mim_solvers
+import argcomplete
def get_args():
diff --git a/python/examples/crocoddyl_ocp_clik.py b/python/examples/crocoddyl_ocp_clik.py
index c062fce6901717262ce8b732a41886f8666b571b..71cc5bf8a8464ba57cef3458bac00bb4ea07bfb0 100644
--- a/python/examples/crocoddyl_ocp_clik.py
+++ b/python/examples/crocoddyl_ocp_clik.py
@@ -11,6 +11,7 @@ from ur_simple_control.util.logging_utils import LogManager
from ur_simple_control.visualize.visualize import plotFromDict
import pinocchio as pin
import crocoddyl
+import argcomplete
def get_args():
@@ -27,6 +28,12 @@ if __name__ == "__main__":
# TODO: put this back for nicer demos
#Mgoal = robot.defineGoalPointCLI()
Mgoal = pin.SE3.Random()
+
+ if args.visualize_manipulator:
+ # TODO document this somewhere
+ robot.manipulator_visualizer_queue.put(
+ {"Mgoal" : Mgoal})
+
# create and solve the optimal control problem of
# getting from current to goal end-effector position.
# reference is position and velocity reference (as a dictionary),
@@ -36,9 +43,12 @@ if __name__ == "__main__":
problem = createCrocoIKOCP(args, robot, x0, Mgoal)
# this shouldn't really depend on x0 but i can't be bothered
reference, solver = solveCrocoOCP(args, robot, problem, x0)
- if args.solver == "boxfddp":
- log = solver.getCallbacks()[1]
- crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=True)
+
+# NOTE: IF YOU PLOT SOMETHING OTHER THAN REAL-TIME PLOTTING FIRST IT BREAKS EVERYTHING
+# if args.solver == "boxfddp":
+# log = solver.getCallbacks()[1]
+# crocoddyl.plotOCSolution(log.xs, log.us, figIndex=1, show=True)
+
# we need a way to follow the reference trajectory,
# both because there can be disturbances,
# and because it is sampled at a much lower frequency
@@ -47,7 +57,6 @@ if __name__ == "__main__":
print("final position:")
print(robot.getT_w_e())
- robot.log_manager.plotAllControlLoops()
if not args.pinocchio_only:
robot.stopRobot()
@@ -57,5 +66,5 @@ if __name__ == "__main__":
if args.save_log:
robot.log_manager.saveLog()
- #loop_manager.stopHandler(None, None)
+ robot.log_manager.plotAllControlLoops()
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index 5c775079e64a0577f7895d5d62755dac14033617..0b12e579a67a1c3b313869c551c96009c53b34dd 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -1,4 +1,5 @@
# PYTHON_ARGCOMPLETE_OK
+# TODO: make 1/beta = 5
import pinocchio as pin
import numpy as np
import matplotlib.pyplot as plt
diff --git a/python/examples/joint_trajectory.csv b/python/examples/joint_trajectory.csv
index ede219bb31636e192cba9c70f654516274b8b89d..1bbd90f9fe36f9f436075faf420246ec085a382a 100644
--- a/python/examples/joint_trajectory.csv
+++ b/python/examples/joint_trajectory.csv
@@ -1,546 +1,173 @@
-0.00000,1.26610,-1.48461,-1.32203,-1.03743,1.81923,-0.19287
-0.03670,1.26607,-1.48462,-1.32205,-1.03740,1.81924,-0.19288
-0.07339,1.26581,-1.48469,-1.32226,-1.03705,1.81932,-0.19305
-0.11009,1.26495,-1.48491,-1.32283,-1.03609,1.81966,-0.19359
-0.14679,1.26236,-1.48486,-1.32559,-1.03307,1.82086,-0.19522
-0.18349,1.26036,-1.48427,-1.32870,-1.03036,1.82183,-0.19648
-0.22018,1.25566,-1.48042,-1.34034,-1.02230,1.82432,-0.19945
-0.25688,1.24957,-1.47160,-1.36218,-1.00927,1.82788,-0.20331
-0.29358,1.24620,-1.46459,-1.37817,-1.00036,1.82992,-0.20543
-0.33028,1.24429,-1.45949,-1.38933,-0.99436,1.83110,-0.20663
-0.36697,1.24168,-1.44745,-1.41408,-0.98175,1.83272,-0.20823
-0.40367,1.23977,-1.43819,-1.43289,-0.97234,1.83398,-0.20941
-0.44037,1.23843,-1.42937,-1.45042,-0.96375,1.83486,-0.21022
-0.47706,1.23760,-1.42350,-1.46200,-0.95812,1.83542,-0.21072
-0.51376,1.23662,-1.41458,-1.47935,-0.94978,1.83608,-0.21130
-0.55046,1.23603,-1.40701,-1.49390,-0.94286,1.83647,-0.21163
-0.58716,1.23552,-1.40111,-1.50523,-0.93748,1.83681,-0.21193
-0.62385,1.23497,-1.39521,-1.51656,-0.93211,1.83720,-0.21225
-0.66055,1.23449,-1.39008,-1.52641,-0.92746,1.83753,-0.21253
-0.69725,1.23414,-1.38725,-1.53188,-0.92487,1.83778,-0.21274
-0.73394,1.23344,-1.38309,-1.54002,-0.92099,1.83829,-0.21317
-0.77064,1.23245,-1.37835,-1.54941,-0.91650,1.83902,-0.21380
-0.80734,1.23126,-1.37216,-1.56158,-0.91071,1.83990,-0.21455
-0.84404,1.22946,-1.36488,-1.57611,-0.90375,1.84123,-0.21569
-0.88073,1.22780,-1.35905,-1.58786,-0.89811,1.84247,-0.21675
-0.91743,1.22622,-1.35395,-1.59821,-0.89312,1.84365,-0.21778
-0.95413,1.22169,-1.33932,-1.62779,-0.87894,1.84703,-0.22070
-0.99083,1.21912,-1.33191,-1.64292,-0.87165,1.84895,-0.22237
-1.02752,1.21697,-1.32499,-1.65687,-0.86499,1.85055,-0.22376
-1.06422,1.21552,-1.32070,-1.66559,-0.86082,1.85163,-0.22470
-1.10092,1.21306,-1.31405,-1.67920,-0.85428,1.85347,-0.22631
-1.13761,1.21021,-1.30693,-1.69389,-0.84722,1.85560,-0.22817
-1.17431,1.20725,-1.29992,-1.70845,-0.84021,1.85782,-0.23012
-1.21101,1.20496,-1.29473,-1.71927,-0.83499,1.85953,-0.23163
-1.24771,1.20288,-1.29015,-1.72885,-0.83037,1.86109,-0.23300
-1.28440,1.19930,-1.28359,-1.74292,-0.82351,1.86377,-0.23537
-1.32110,1.19606,-1.27759,-1.75578,-0.81727,1.86620,-0.23752
-1.35780,1.19215,-1.27130,-1.76957,-0.81051,1.86913,-0.24013
-1.39450,1.18867,-1.26550,-1.78218,-0.80436,1.87173,-0.24245
-1.43119,1.18519,-1.26044,-1.79348,-0.79880,1.87434,-0.24478
-1.46789,1.17943,-1.25253,-1.81133,-0.78998,1.87865,-0.24865
-1.50459,1.17008,-1.23982,-1.84007,-0.77583,1.88564,-0.25496
-1.54128,1.16726,-1.23590,-1.84891,-0.77149,1.88775,-0.25687
-1.57798,1.16315,-1.23000,-1.86209,-0.76505,1.89081,-0.25965
-1.61468,1.15942,-1.22456,-1.87420,-0.75916,1.89360,-0.26218
-1.65138,1.15638,-1.22013,-1.88408,-0.75436,1.89586,-0.26425
-1.68807,1.15369,-1.21622,-1.89278,-0.75014,1.89787,-0.26608
-1.72477,1.15191,-1.21312,-1.89942,-0.74696,1.89919,-0.26729
-1.76147,1.14459,-1.20165,-1.92454,-0.73491,1.90462,-0.27229
-1.79817,1.14128,-1.19675,-1.93539,-0.72968,1.90708,-0.27455
-1.83486,1.13860,-1.19290,-1.94395,-0.72555,1.90907,-0.27640
-1.87156,1.13688,-1.19050,-1.94934,-0.72295,1.91035,-0.27758
-1.90826,1.13551,-1.18860,-1.95360,-0.72090,1.91137,-0.27852
-1.94495,1.13447,-1.18719,-1.95678,-0.71936,1.91214,-0.27924
-1.98165,1.13412,-1.18671,-1.95785,-0.71884,1.91240,-0.27948
-2.01835,1.13342,-1.18577,-1.95998,-0.71782,1.91291,-0.27996
-2.05505,1.12989,-1.18178,-1.96939,-0.71320,1.91554,-0.28241
-2.09174,1.12755,-1.17913,-1.97564,-0.71013,1.91728,-0.28403
-2.12844,1.12484,-1.17603,-1.98292,-0.70657,1.91928,-0.28590
-2.16514,1.12252,-1.17337,-1.98916,-0.70353,1.92100,-0.28751
-2.20183,1.11946,-1.16985,-1.99743,-0.69950,1.92327,-0.28964
-2.23853,1.11868,-1.16897,-1.99951,-0.69848,1.92384,-0.29018
-2.27523,1.11659,-1.16879,-2.00133,-0.69733,1.92540,-0.29165
-2.31193,1.11052,-1.16998,-2.00371,-0.69521,1.92992,-0.29593
-2.34862,1.09841,-1.17433,-2.00518,-0.69237,1.93892,-0.30452
-2.38532,1.08942,-1.17820,-2.00526,-0.69070,1.94559,-0.31094
-2.42202,1.07881,-1.18341,-2.00431,-0.68919,1.95345,-0.31856
-2.45872,1.06085,-1.19312,-2.00138,-0.68724,1.96672,-0.33158
-2.49541,1.05015,-1.19937,-1.99895,-0.68640,1.97459,-0.33940
-2.53211,1.03928,-1.20590,-1.99626,-0.68567,1.98258,-0.34741
-2.56881,1.02983,-1.21167,-1.99383,-0.68510,1.98949,-0.35441
-2.60550,1.02449,-1.21499,-1.99238,-0.68481,1.99340,-0.35839
-2.64220,1.02195,-1.21660,-1.99164,-0.68471,1.99526,-0.36029
-2.67890,1.01824,-1.21903,-1.99044,-0.68460,1.99796,-0.36307
-2.71560,1.01698,-1.21987,-1.99002,-0.68457,1.99888,-0.36401
-2.75229,1.01402,-1.22394,-1.98530,-0.68612,2.00105,-0.36625
-2.78899,1.01100,-1.23343,-1.97100,-0.69188,2.00329,-0.36856
-2.82569,1.02039,-1.25563,-1.92344,-0.71441,1.99661,-0.36169
-2.86239,1.02986,-1.26423,-1.90013,-0.72632,1.98977,-0.35472
-2.89908,1.04275,-1.27425,-1.87159,-0.74112,1.98041,-0.34528
-2.93578,1.05329,-1.28265,-1.84799,-0.75336,1.97274,-0.33763
-2.97248,1.06354,-1.29425,-1.81892,-0.76802,1.96526,-0.33025
-3.00917,1.06747,-1.30145,-1.80278,-0.77592,1.96240,-0.32745
-3.04587,1.07239,-1.31301,-1.77792,-0.78792,1.95882,-0.32397
-3.08257,1.07615,-1.32259,-1.75751,-0.79778,1.95610,-0.32133
-3.11927,1.07884,-1.32931,-1.74313,-0.80474,1.95414,-0.31944
-3.15596,1.08333,-1.33840,-1.72302,-0.81462,1.95087,-0.31628
-3.19266,1.09513,-1.35926,-1.67572,-0.83809,1.94222,-0.30800
-3.22936,1.10105,-1.36986,-1.65169,-0.85005,1.93788,-0.30387
-3.26606,1.10667,-1.38007,-1.62862,-0.86155,1.93374,-0.29996
-3.30275,1.10958,-1.38632,-1.61488,-0.86835,1.93161,-0.29795
-3.33945,1.11201,-1.39138,-1.60369,-0.87390,1.92982,-0.29628
-3.37615,1.11391,-1.39532,-1.59496,-0.87824,1.92842,-0.29497
-3.41284,1.11446,-1.39645,-1.59246,-0.87948,1.92802,-0.29459
-3.44954,1.11473,-1.39702,-1.59120,-0.88011,1.92782,-0.29440
-3.48624,1.11611,-1.39986,-1.58490,-0.88324,1.92680,-0.29345
-3.52294,1.12889,-1.42087,-1.53654,-0.90763,1.91736,-0.28465
-3.55963,1.13194,-1.42649,-1.52384,-0.91402,1.91510,-0.28256
-3.59633,1.13574,-1.43400,-1.50708,-0.92243,1.91229,-0.27997
-3.63303,1.13745,-1.43750,-1.49929,-0.92633,1.91103,-0.27881
-3.66972,1.13830,-1.43929,-1.49535,-0.92831,1.91040,-0.27823
-3.70642,1.13913,-1.44105,-1.49145,-0.93025,1.90978,-0.27767
-3.74312,1.13997,-1.44285,-1.48748,-0.93224,1.90916,-0.27710
-3.77982,1.14108,-1.44526,-1.48216,-0.93490,1.90834,-0.27635
-3.81651,1.14297,-1.44948,-1.47291,-0.93953,1.90694,-0.27507
-3.85321,1.14537,-1.45582,-1.45928,-0.94629,1.90517,-0.27346
-3.88991,1.14773,-1.46302,-1.44405,-0.95382,1.90342,-0.27188
-3.92661,1.15046,-1.47084,-1.42739,-0.96209,1.90141,-0.27006
-3.96330,1.15316,-1.47957,-1.40898,-0.97119,1.89942,-0.26826
-4.00000,1.15505,-1.48648,-1.39458,-0.97830,1.89802,-0.26701
-4.03670,1.15625,-1.49060,-1.38592,-0.98258,1.89714,-0.26622
-4.07339,1.15728,-1.49402,-1.37871,-0.98615,1.89638,-0.26554
-4.11009,1.15811,-1.49673,-1.37299,-0.98900,1.89576,-0.26498
-4.14679,1.15875,-1.49879,-1.36863,-0.99116,1.89529,-0.26456
-4.18349,1.15963,-1.50154,-1.36281,-0.99406,1.89464,-0.26398
-4.22018,1.16073,-1.50496,-1.35555,-0.99767,1.89382,-0.26325
-4.25688,1.16208,-1.50908,-1.34679,-1.00204,1.89283,-0.26235
-4.29358,1.16344,-1.51321,-1.33799,-1.00643,1.89182,-0.26145
-4.33028,1.16412,-1.51528,-1.33357,-1.00864,1.89131,-0.26100
-4.36697,1.16457,-1.51666,-1.33065,-1.01009,1.89098,-0.26071
-4.40367,1.16502,-1.51803,-1.32772,-1.01156,1.89065,-0.26041
-4.44037,1.16525,-1.51872,-1.32624,-1.01229,1.89048,-0.26026
-4.47706,1.16548,-1.51942,-1.32476,-1.01303,1.89031,-0.26011
-4.51376,1.16597,-1.52173,-1.32002,-1.01537,1.88995,-0.25979
-4.55046,1.16618,-1.52452,-1.31452,-1.01804,1.88980,-0.25967
-4.58716,1.16608,-1.52687,-1.31008,-1.02016,1.88988,-0.25975
-4.62385,1.16535,-1.53046,-1.30366,-1.02315,1.89044,-0.26028
-4.66055,1.16427,-1.53408,-1.29739,-1.02603,1.89127,-0.26105
-4.69725,1.16300,-1.53729,-1.29205,-1.02844,1.89223,-0.26195
-4.73394,1.16088,-1.54153,-1.28526,-1.03145,1.89385,-0.26344
-4.77064,1.15849,-1.54545,-1.27929,-1.03403,1.89566,-0.26512
-4.80734,1.15619,-1.54876,-1.27439,-1.03611,1.89740,-0.26673
-4.84404,1.15362,-1.55215,-1.26956,-1.03813,1.89934,-0.26853
-4.88073,1.15079,-1.55558,-1.26480,-1.04008,1.90148,-0.27051
-4.91743,1.14874,-1.55783,-1.26180,-1.04129,1.90302,-0.27194
-4.95413,1.14649,-1.56012,-1.25887,-1.04244,1.90472,-0.27352
-4.99083,1.14412,-1.56235,-1.25613,-1.04348,1.90651,-0.27518
-5.02752,1.14070,-1.56536,-1.25261,-1.04477,1.90908,-0.27758
-5.06422,1.13718,-1.56827,-1.24934,-1.04594,1.91173,-0.28005
-5.10092,1.13175,-1.57272,-1.24439,-1.04769,1.91581,-0.28387
-5.13761,1.12903,-1.57496,-1.24191,-1.04857,1.91786,-0.28579
-5.17431,1.12453,-1.57865,-1.23784,-1.05002,1.92123,-0.28897
-5.21101,1.12187,-1.58085,-1.23542,-1.05088,1.92323,-0.29086
-5.24771,1.12097,-1.58158,-1.23461,-1.05117,1.92390,-0.29149
-5.28440,1.11915,-1.58308,-1.23296,-1.05176,1.92526,-0.29278
-5.32110,1.11738,-1.58455,-1.23135,-1.05233,1.92659,-0.29404
-5.35780,1.11649,-1.58528,-1.23054,-1.05262,1.92726,-0.29467
-5.39450,1.11560,-1.58601,-1.22973,-1.05291,1.92792,-0.29530
-5.43119,1.11472,-1.58675,-1.22893,-1.05320,1.92859,-0.29593
-5.46789,1.11468,-1.58678,-1.22889,-1.05321,1.92862,-0.29596
-5.50459,1.11281,-1.58810,-1.22763,-1.05361,1.93002,-0.29729
-5.54128,1.10503,-1.58871,-1.23191,-1.05064,1.93581,-0.30279
-5.57798,1.09907,-1.58698,-1.23948,-1.04629,1.94023,-0.30700
-5.61468,1.09315,-1.58315,-1.25116,-1.03994,1.94462,-0.31119
-5.65138,1.08942,-1.57985,-1.26025,-1.03511,1.94737,-0.31382
-5.68807,1.08480,-1.57440,-1.27418,-1.02781,1.95078,-0.31708
-5.72477,1.07860,-1.56490,-1.29715,-1.01595,1.95533,-0.32145
-5.76147,1.07517,-1.55909,-1.31096,-1.00885,1.95785,-0.32388
-5.79817,1.07254,-1.55437,-1.32205,-1.00317,1.95977,-0.32573
-5.83486,1.07062,-1.55078,-1.33043,-0.99888,1.96118,-0.32709
-5.87156,1.06662,-1.54298,-1.34851,-0.98966,1.96409,-0.32991
-5.90826,1.06331,-1.53639,-1.36376,-0.98189,1.96650,-0.33226
-5.94495,1.06003,-1.52983,-1.37892,-0.97418,1.96889,-0.33458
-5.98165,1.05826,-1.52629,-1.38708,-0.97002,1.97018,-0.33584
-6.01835,1.05618,-1.52216,-1.39664,-0.96516,1.97169,-0.33732
-6.05505,1.05499,-1.51981,-1.40209,-0.96238,1.97255,-0.33816
-6.09174,1.05293,-1.51575,-1.41149,-0.95760,1.97405,-0.33963
-6.12844,1.05025,-1.51054,-1.42361,-0.95144,1.97599,-0.34154
-6.16514,1.04673,-1.50524,-1.43654,-0.94479,1.97855,-0.34407
-6.20183,1.04353,-1.50129,-1.44664,-0.93954,1.98088,-0.34638
-6.23853,1.03958,-1.49649,-1.45898,-0.93312,1.98375,-0.34925
-6.27523,1.03713,-1.49370,-1.46628,-0.92931,1.98553,-0.35103
-6.31193,1.03383,-1.49003,-1.47595,-0.92427,1.98792,-0.35343
-6.34862,1.02935,-1.48501,-1.48916,-0.91737,1.99117,-0.35670
-6.38532,1.01983,-1.47327,-1.51934,-0.90175,1.99804,-0.36368
-6.42202,1.01747,-1.46983,-1.52786,-0.89739,1.99975,-0.36541
-6.45872,1.01576,-1.46734,-1.53403,-0.89422,2.00098,-0.36666
-6.49541,1.01361,-1.46369,-1.54275,-0.88979,2.00252,-0.36824
-6.53211,1.01232,-1.46078,-1.54937,-0.88647,2.00344,-0.36918
-6.56881,1.01089,-1.45727,-1.55725,-0.88254,2.00447,-0.37023
-6.60550,1.00968,-1.45350,-1.56541,-0.87850,2.00533,-0.37110
-6.64220,1.00828,-1.44839,-1.57629,-0.87315,2.00632,-0.37212
-6.67890,1.00681,-1.44258,-1.58854,-0.86714,2.00736,-0.37318
-6.71560,1.00630,-1.43975,-1.59435,-0.86432,2.00772,-0.37354
-6.75229,1.00581,-1.43614,-1.60159,-0.86083,2.00806,-0.37388
-6.78899,1.00488,-1.43009,-1.61383,-0.85491,2.00870,-0.37453
-6.82569,1.00458,-1.42707,-1.61979,-0.85205,2.00891,-0.37473
-6.86239,1.00395,-1.42154,-1.63077,-0.84679,2.00934,-0.37517
-6.89908,1.00353,-1.41726,-1.63920,-0.84275,2.00962,-0.37545
-6.93578,1.00270,-1.41216,-1.64952,-0.83778,2.01020,-0.37604
-6.97248,1.00212,-1.40890,-1.65615,-0.83458,2.01060,-0.37645
-7.00917,1.00134,-1.40510,-1.66397,-0.83079,2.01115,-0.37701
-7.04587,1.00090,-1.40333,-1.66767,-0.82900,2.01146,-0.37733
-7.08257,1.00012,-1.40026,-1.67411,-0.82587,2.01201,-0.37789
-7.11927,0.99960,-1.39852,-1.67783,-0.82405,2.01237,-0.37827
-7.15596,0.99923,-1.39739,-1.68025,-0.82286,2.01263,-0.37854
-7.19266,0.99817,-1.39466,-1.68628,-0.81989,2.01339,-0.37932
-7.22936,0.99643,-1.39096,-1.69472,-0.81570,2.01462,-0.38061
-7.26606,0.99424,-1.38693,-1.70415,-0.81098,2.01618,-0.38224
-7.30275,0.99187,-1.38306,-1.71345,-0.80630,2.01787,-0.38401
-7.33945,0.98948,-1.37996,-1.72131,-0.80230,2.01958,-0.38580
-7.37615,0.98772,-1.37774,-1.72699,-0.79941,2.02083,-0.38712
-7.41284,0.98668,-1.37644,-1.73033,-0.79770,2.02158,-0.38790
-7.44954,0.98597,-1.37557,-1.73258,-0.79655,2.02208,-0.38844
-7.48624,0.98489,-1.37426,-1.73596,-0.79482,2.02285,-0.38925
-7.52294,0.98346,-1.37255,-1.74041,-0.79255,2.02387,-0.39032
-7.55963,0.98189,-1.37096,-1.74474,-0.79031,2.02499,-0.39151
-7.59633,0.97837,-1.36737,-1.75454,-0.78526,2.02750,-0.39418
-7.63303,0.97526,-1.36418,-1.76323,-0.78078,2.02971,-0.39654
-7.66972,0.97029,-1.35971,-1.77598,-0.77416,2.03325,-0.40033
-7.70642,0.96824,-1.35786,-1.78125,-0.77142,2.03471,-0.40189
-7.74312,0.96340,-1.35337,-1.79396,-0.76485,2.03814,-0.40560
-7.77982,0.96077,-1.35128,-1.80022,-0.76157,2.04001,-0.40763
-7.81651,0.95820,-1.34918,-1.80644,-0.75833,2.04183,-0.40960
-7.85321,0.95651,-1.34778,-1.81058,-0.75617,2.04303,-0.41091
-7.88991,0.95441,-1.34602,-1.81576,-0.75348,2.04451,-0.41253
-7.92661,0.95316,-1.34497,-1.81885,-0.75187,2.04540,-0.41350
-7.96330,0.95149,-1.34356,-1.82300,-0.74971,2.04658,-0.41479
-8.00000,0.94879,-1.34159,-1.82915,-0.74648,2.04849,-0.41689
-8.03670,0.94731,-1.34068,-1.83220,-0.74487,2.04953,-0.41803
-8.07339,0.94591,-1.33978,-1.83515,-0.74330,2.05052,-0.41912
-8.11009,0.94453,-1.33888,-1.83811,-0.74173,2.05149,-0.42020
-8.14679,0.94317,-1.33795,-1.84109,-0.74016,2.05246,-0.42127
-8.18349,0.94314,-1.33794,-1.84114,-0.74014,2.05247,-0.42128
-8.22018,0.94271,-1.33764,-1.84210,-0.73963,2.05278,-0.42162
-8.25688,0.94225,-1.33733,-1.84310,-0.73910,2.05310,-0.42198
-8.29358,0.94179,-1.33701,-1.84411,-0.73857,2.05343,-0.42234
-8.33028,0.93916,-1.33569,-1.84902,-0.73593,2.05528,-0.42440
-8.36697,0.93437,-1.33408,-1.85652,-0.73177,2.05866,-0.42817
-8.40367,0.92933,-1.33326,-1.86281,-0.72814,2.06221,-0.43215
-8.44037,0.92694,-1.33305,-1.86548,-0.72656,2.06389,-0.43404
-8.47706,0.92477,-1.33288,-1.86789,-0.72513,2.06542,-0.43577
-8.51376,0.92413,-1.33282,-1.86861,-0.72470,2.06587,-0.43628
-8.55046,0.92349,-1.33275,-1.86935,-0.72427,2.06632,-0.43678
-8.58716,0.92287,-1.33267,-1.87010,-0.72383,2.06675,-0.43728
-8.62385,0.92225,-1.33259,-1.87085,-0.72340,2.06719,-0.43777
-8.66055,0.92164,-1.33249,-1.87162,-0.72295,2.06762,-0.43826
-8.69725,0.92080,-1.33240,-1.87260,-0.72238,2.06821,-0.43893
-8.73394,0.91835,-1.33229,-1.87518,-0.72084,2.06993,-0.44088
-8.77064,0.91676,-1.33225,-1.87680,-0.71986,2.07104,-0.44216
-8.80734,0.91515,-1.33224,-1.87838,-0.71889,2.07217,-0.44345
-8.84404,0.91511,-1.33224,-1.87842,-0.71887,2.07220,-0.44348
-8.88073,0.91451,-1.33222,-1.87903,-0.71850,2.07262,-0.44396
-8.91743,0.91385,-1.33220,-1.87972,-0.71808,2.07308,-0.44449
-8.95413,0.91319,-1.33216,-1.88042,-0.71766,2.07354,-0.44501
-8.99083,0.91260,-1.33219,-1.88095,-0.71733,2.07395,-0.44549
-9.02752,0.91022,-1.33264,-1.88245,-0.71629,2.07562,-0.44741
-9.06422,0.90511,-1.33470,-1.88369,-0.71496,2.07920,-0.45154
-9.10092,0.90031,-1.33746,-1.88338,-0.71439,2.08257,-0.45544
-9.13761,0.89409,-1.34174,-1.88171,-0.71424,2.08691,-0.46052
-9.17431,0.88880,-1.34595,-1.87926,-0.71459,2.09061,-0.46487
-9.21101,0.88408,-1.35001,-1.87654,-0.71516,2.09389,-0.46876
-9.24771,0.88116,-1.35258,-1.87475,-0.71557,2.09593,-0.47117
-9.28440,0.87744,-1.35605,-1.87213,-0.71625,2.09851,-0.47426
-9.32110,0.87463,-1.35872,-1.87008,-0.71680,2.10047,-0.47661
-9.35780,0.87276,-1.36050,-1.86869,-0.71718,2.10177,-0.47817
-9.39450,0.87184,-1.36141,-1.86794,-0.71739,2.10240,-0.47893
-9.43119,0.87093,-1.36233,-1.86720,-0.71761,2.10304,-0.47969
-9.46789,0.86826,-1.36509,-1.86483,-0.71834,2.10488,-0.48193
-9.50459,0.86558,-1.36797,-1.86226,-0.71915,2.10674,-0.48418
-9.54128,0.86299,-1.37075,-1.85978,-0.71994,2.10853,-0.48636
-9.57798,0.86038,-1.37365,-1.85711,-0.72083,2.11034,-0.48856
-9.61468,0.85869,-1.37553,-1.85539,-0.72140,2.11150,-0.48999
-9.65138,0.85784,-1.37647,-1.85453,-0.72168,2.11209,-0.49070
-9.68807,0.85700,-1.37741,-1.85367,-0.72197,2.11267,-0.49142
-9.72477,0.85615,-1.37834,-1.85281,-0.72225,2.11325,-0.49214
-9.76147,0.85589,-1.37893,-1.85199,-0.72260,2.11343,-0.49236
-9.79817,0.85543,-1.38455,-1.84193,-0.72725,2.11378,-0.49279
-9.83486,0.85846,-1.39337,-1.82212,-0.73698,2.11177,-0.49031
-9.87156,0.86403,-1.40102,-1.80180,-0.74731,2.10800,-0.48571
-9.90826,0.86803,-1.40566,-1.78885,-0.75396,2.10528,-0.48242
-9.94495,0.87685,-1.41438,-1.76323,-0.76724,2.09926,-0.47518
-9.98165,0.88711,-1.42412,-1.73432,-0.78228,2.09222,-0.46682
-10.01835,0.89165,-1.42852,-1.72140,-0.78900,2.08909,-0.46314
-10.05505,0.90578,-1.44465,-1.67673,-0.81204,2.07933,-0.45179
-10.09174,0.90754,-1.44673,-1.67105,-0.81497,2.07811,-0.45039
-10.12844,0.91574,-1.45697,-1.64357,-0.82912,2.07241,-0.44388
-10.16514,0.91996,-1.46284,-1.62829,-0.83695,2.06948,-0.44055
-10.20183,0.92338,-1.46802,-1.61512,-0.84368,2.06710,-0.43786
-10.23853,0.92605,-1.47241,-1.60418,-0.84925,2.06524,-0.43576
-10.27523,0.92833,-1.47644,-1.59431,-0.85426,2.06365,-0.43399
-10.31193,0.93202,-1.48300,-1.57827,-0.86241,2.06108,-0.43112
-10.34862,0.93596,-1.48932,-1.56243,-0.87050,2.05832,-0.42805
-10.38532,0.93888,-1.49442,-1.54992,-0.87687,2.05628,-0.42579
-10.42202,0.94168,-1.49963,-1.53732,-0.88328,2.05432,-0.42363
-10.45872,0.94500,-1.50601,-1.52199,-0.89107,2.05200,-0.42108
-10.49541,0.94791,-1.51119,-1.50933,-0.89752,2.04995,-0.41884
-10.53211,0.95062,-1.51580,-1.49797,-0.90333,2.04805,-0.41676
-10.56881,0.95656,-1.52544,-1.47397,-0.91564,2.04386,-0.41221
-10.60550,0.95939,-1.53006,-1.46251,-0.92152,2.04187,-0.41006
-10.64220,0.96350,-1.53678,-1.44582,-0.93009,2.03896,-0.40692
-10.67890,0.96690,-1.54177,-1.43313,-0.93665,2.03656,-0.40434
-10.71560,0.96956,-1.54585,-1.42287,-0.94194,2.03467,-0.40232
-10.75229,0.97187,-1.54950,-1.41377,-0.94663,2.03304,-0.40058
-10.78899,0.97413,-1.55316,-1.40467,-0.95131,2.03143,-0.39888
-10.82569,0.97733,-1.55852,-1.39149,-0.95809,2.02916,-0.39647
-10.86239,0.97987,-1.56289,-1.38078,-0.96360,2.02735,-0.39456
-10.89908,0.98300,-1.56840,-1.36736,-0.97049,2.02513,-0.39222
-10.93578,0.98537,-1.57392,-1.35459,-0.97698,2.02344,-0.39046
-10.97248,0.98765,-1.58048,-1.33987,-0.98442,2.02183,-0.38877
-11.00917,0.98988,-1.58899,-1.32139,-0.99369,2.02025,-0.38715
-11.04587,0.99119,-1.59594,-1.30674,-1.00100,2.01934,-0.38622
-11.08257,0.99205,-1.60038,-1.29734,-1.00570,2.01874,-0.38561
-11.11927,0.99293,-1.60488,-1.28781,-1.01046,2.01812,-0.38498
-11.15596,0.99350,-1.60798,-1.28126,-1.01373,2.01772,-0.38457
-11.19266,0.99378,-1.60941,-1.27823,-1.01525,2.01752,-0.38437
-11.22936,0.99436,-1.61252,-1.27165,-1.01854,2.01712,-0.38396
-11.26606,0.99499,-1.61538,-1.26554,-1.02161,2.01667,-0.38351
-11.30275,0.99579,-1.61920,-1.25738,-1.02570,2.01611,-0.38294
-11.33945,0.99686,-1.62346,-1.24813,-1.03036,2.01536,-0.38217
-11.37615,0.99744,-1.62562,-1.24342,-1.03274,2.01495,-0.38175
-11.41284,0.99825,-1.62842,-1.23723,-1.03587,2.01437,-0.38116
-11.44954,0.99912,-1.63124,-1.23098,-1.03904,2.01375,-0.38053
-11.48624,0.99934,-1.63194,-1.22942,-1.03984,2.01360,-0.38037
-11.52294,0.99956,-1.63264,-1.22786,-1.04063,2.01344,-0.38021
-11.55963,1.00151,-1.63909,-1.21357,-1.04787,2.01205,-0.37880
-11.59633,1.00288,-1.64334,-1.20410,-1.05268,2.01108,-0.37781
-11.63303,1.00334,-1.64474,-1.20096,-1.05428,2.01075,-0.37748
-11.66972,1.00382,-1.64618,-1.19774,-1.05592,2.01041,-0.37713
-11.70642,1.00477,-1.64895,-1.19149,-1.05910,2.00973,-0.37644
-11.74312,1.00478,-1.64899,-1.19141,-1.05914,2.00973,-0.37643
-11.77982,1.00502,-1.64969,-1.18983,-1.05995,2.00955,-0.37625
-11.81651,1.00526,-1.65039,-1.18825,-1.06075,2.00938,-0.37608
-11.85321,1.00551,-1.65109,-1.18667,-1.06156,2.00921,-0.37590
-11.88991,1.00575,-1.65179,-1.18509,-1.06237,2.00903,-0.37572
-11.92661,1.00600,-1.65249,-1.18351,-1.06317,2.00886,-0.37555
-11.96330,1.00625,-1.65319,-1.18193,-1.06398,2.00868,-0.37537
-12.00000,1.00650,-1.65389,-1.18034,-1.06479,2.00850,-0.37518
-12.03670,1.00691,-1.65704,-1.17382,-1.06805,2.00821,-0.37490
-12.07339,1.00682,-1.66041,-1.16727,-1.07127,2.00829,-0.37500
-12.11009,1.00618,-1.66370,-1.16133,-1.07413,2.00876,-0.37551
-12.14679,1.00401,-1.66981,-1.15111,-1.07894,2.01036,-0.37720
-12.18349,1.00114,-1.67601,-1.14131,-1.08348,2.01246,-0.37942
-12.22018,0.99822,-1.68120,-1.13353,-1.08701,2.01459,-0.38167
-12.25688,0.99476,-1.68654,-1.12592,-1.09042,2.01710,-0.38434
-12.29358,0.98955,-1.69340,-1.11679,-1.09441,2.02089,-0.38835
-12.33028,0.98676,-1.69687,-1.11232,-1.09633,2.02291,-0.39051
-12.36697,0.98391,-1.70022,-1.10814,-1.09811,2.02498,-0.39271
-12.40367,0.98014,-1.70427,-1.10339,-1.10009,2.02770,-0.39563
-12.44037,0.97551,-1.70891,-1.09821,-1.10219,2.03105,-0.39923
-12.47706,0.97075,-1.71344,-1.09341,-1.10410,2.03447,-0.40292
-12.51376,0.96749,-1.71636,-1.09048,-1.10522,2.03682,-0.40547
-12.55046,0.96282,-1.72063,-1.08616,-1.10690,2.04018,-0.40912
-12.58716,0.96045,-1.72277,-1.08403,-1.10773,2.04188,-0.41098
-12.62385,0.95893,-1.72418,-1.08258,-1.10830,2.04297,-0.41217
-12.66055,0.95817,-1.72490,-1.08184,-1.10859,2.04352,-0.41277
-12.69725,0.95742,-1.72562,-1.08108,-1.10889,2.04406,-0.41336
-12.73394,0.95669,-1.72631,-1.08035,-1.10918,2.04457,-0.41393
-12.77064,0.95594,-1.72705,-1.07957,-1.10950,2.04511,-0.41453
-12.80734,0.95508,-1.72786,-1.07874,-1.10982,2.04573,-0.41521
-12.84404,0.95494,-1.72798,-1.07863,-1.10987,2.04583,-0.41531
-12.88073,0.95269,-1.72958,-1.07747,-1.11022,2.04744,-0.41708
-12.91743,0.95023,-1.73046,-1.07794,-1.10975,2.04920,-0.41901
-12.95413,0.94603,-1.72968,-1.08327,-1.10670,2.05218,-0.42230
-12.99083,0.93646,-1.71853,-1.11408,-1.09050,2.05894,-0.42974
-13.02752,0.93139,-1.70986,-1.13594,-1.07915,2.06250,-0.43367
-13.06422,0.92794,-1.70301,-1.15270,-1.07049,2.06491,-0.43635
-13.10092,0.91549,-1.67876,-1.21240,-1.03963,2.07358,-0.44606
-13.13761,0.91236,-1.67292,-1.22694,-1.03209,2.07575,-0.44851
-13.17431,0.90924,-1.66719,-1.24126,-1.02468,2.07792,-0.45096
-13.21101,0.90322,-1.65559,-1.26997,-1.00984,2.08208,-0.45571
-13.24771,0.89974,-1.64857,-1.28721,-1.00096,2.08448,-0.45847
-13.28440,0.89644,-1.64241,-1.30259,-0.99301,2.08676,-0.46109
-13.32110,0.89356,-1.63672,-1.31664,-0.98576,2.08874,-0.46338
-13.35780,0.88680,-1.62406,-1.34824,-0.96948,2.09338,-0.46879
-13.39450,0.88147,-1.61437,-1.37263,-0.95690,2.09704,-0.47308
-13.43119,0.87902,-1.61026,-1.38318,-0.95144,2.09871,-0.47505
-13.46789,0.87357,-1.60191,-1.40509,-0.94008,2.10243,-0.47948
-13.50459,0.86519,-1.59114,-1.43482,-0.92457,2.10815,-0.48635
-13.54128,0.86309,-1.58851,-1.44215,-0.92074,2.10958,-0.48807
-13.57798,0.85899,-1.58225,-1.45866,-0.91220,2.11237,-0.49144
-13.61468,0.85328,-1.57296,-1.48279,-0.89975,2.11623,-0.49615
-13.65138,0.84950,-1.56558,-1.50120,-0.89034,2.11878,-0.49928
-13.68807,0.84749,-1.56119,-1.51189,-0.88488,2.12013,-0.50094
-13.72477,0.84274,-1.55111,-1.53659,-0.87231,2.12331,-0.50487
-13.76147,0.83701,-1.54166,-1.56114,-0.85969,2.12715,-0.50967
-13.79817,0.83107,-1.53246,-1.58547,-0.84718,2.13112,-0.51467
-13.83486,0.82742,-1.52717,-1.59974,-0.83981,2.13356,-0.51776
-13.87156,0.82367,-1.52200,-1.61392,-0.83249,2.13606,-0.52094
-13.90826,0.82021,-1.51736,-1.62674,-0.82587,2.13836,-0.52389
-13.94495,0.81789,-1.51459,-1.63471,-0.82173,2.13991,-0.52587
-13.98165,0.81444,-1.50991,-1.64763,-0.81506,2.14219,-0.52883
-14.01835,0.80485,-1.50023,-1.67719,-0.79963,2.14855,-0.53712
-14.05505,0.80153,-1.49722,-1.68681,-0.79458,2.15075,-0.54001
-14.09174,0.80003,-1.49591,-1.69106,-0.79234,2.15174,-0.54132
-14.12844,0.79773,-1.49400,-1.69740,-0.78900,2.15326,-0.54333
-14.16514,0.79656,-1.49306,-1.70059,-0.78732,2.15403,-0.54435
-14.20183,0.79580,-1.49245,-1.70263,-0.78624,2.15453,-0.54501
-14.23853,0.79502,-1.49184,-1.70472,-0.78514,2.15504,-0.54570
-14.27523,0.79304,-1.49032,-1.70994,-0.78238,2.15634,-0.54744
-14.31193,0.79225,-1.48973,-1.71201,-0.78128,2.15686,-0.54813
-14.34862,0.79066,-1.48856,-1.71612,-0.77911,2.15791,-0.54953
-14.38532,0.78987,-1.48798,-1.71817,-0.77802,2.15843,-0.55023
-14.42202,0.78948,-1.48770,-1.71915,-0.77750,2.15868,-0.55057
-14.45872,0.78733,-1.48642,-1.72415,-0.77483,2.16010,-0.55247
-14.49541,0.78554,-1.48553,-1.72798,-0.77276,2.16128,-0.55406
-14.53211,0.78270,-1.48433,-1.73364,-0.76968,2.16313,-0.55658
-14.56881,0.78224,-1.48413,-1.73458,-0.76917,2.16344,-0.55700
-14.60550,0.78131,-1.48372,-1.73646,-0.76816,2.16405,-0.55782
-14.64220,0.78085,-1.48352,-1.73741,-0.76764,2.16435,-0.55823
-14.67890,0.78041,-1.48332,-1.73831,-0.76715,2.16464,-0.55862
-14.71560,0.77995,-1.48311,-1.73926,-0.76664,2.16494,-0.55904
-14.75229,0.77949,-1.48291,-1.74021,-0.76613,2.16524,-0.55945
-14.78899,0.77301,-1.48610,-1.74187,-0.76450,2.16950,-0.56528
-14.82569,0.76494,-1.49174,-1.74085,-0.76398,2.17479,-0.57261
-14.86239,0.76060,-1.49501,-1.73989,-0.76390,2.17763,-0.57658
-14.89908,0.75079,-1.50357,-1.73555,-0.76479,2.18403,-0.58562
-14.93578,0.73681,-1.51714,-1.72697,-0.76726,2.19309,-0.59865
-14.97248,0.72762,-1.52696,-1.71972,-0.76970,2.19900,-0.60732
-15.00917,0.72240,-1.53284,-1.71507,-0.77136,2.20235,-0.61228
-15.04587,0.71866,-1.53715,-1.71158,-0.77263,2.20474,-0.61585
-15.08257,0.71581,-1.54057,-1.70867,-0.77373,2.20655,-0.61858
-15.11927,0.71235,-1.54489,-1.70481,-0.77522,2.20875,-0.62189
-15.15596,0.71029,-1.54753,-1.70238,-0.77617,2.21007,-0.62388
-15.19266,0.70693,-1.55199,-1.69817,-0.77784,2.21220,-0.62713
-15.22936,0.70363,-1.55648,-1.69381,-0.77961,2.21429,-0.63033
-15.26606,0.70036,-1.56105,-1.68928,-0.78146,2.21636,-0.63351
-15.30275,0.69907,-1.56287,-1.68746,-0.78221,2.21717,-0.63477
-15.33945,0.69904,-1.56291,-1.68742,-0.78223,2.21719,-0.63480
-15.37615,0.69843,-1.56378,-1.68654,-0.78259,2.21758,-0.63539
-15.41284,0.69779,-1.56470,-1.68562,-0.78297,2.21798,-0.63602
-15.44954,0.69715,-1.56561,-1.68469,-0.78335,2.21838,-0.63664
-15.48624,0.69651,-1.56653,-1.68376,-0.78374,2.21879,-0.63727
-15.52294,0.69475,-1.56938,-1.68057,-0.78511,2.21990,-0.63899
-15.55963,0.69243,-1.57776,-1.66736,-0.79133,2.22139,-0.64132
-15.59633,0.69203,-1.58156,-1.66047,-0.79467,2.22166,-0.64174
-15.63303,0.69546,-1.59039,-1.63851,-0.80585,2.21959,-0.63850
-15.66972,0.70130,-1.59628,-1.61908,-0.81607,2.21599,-0.63293
-15.70642,0.70544,-1.59911,-1.60798,-0.82199,2.21341,-0.62899
-15.74312,0.71609,-1.60502,-1.58232,-0.83579,2.20675,-0.61890
-15.77982,0.72288,-1.60936,-1.56495,-0.84507,2.20248,-0.61252
-15.81651,0.72758,-1.61261,-1.55252,-0.85170,2.19952,-0.60813
-15.85321,0.73492,-1.61806,-1.53244,-0.86238,2.19487,-0.60132
-15.88991,0.73987,-1.62198,-1.51846,-0.86980,2.19171,-0.59674
-15.92661,0.74365,-1.62509,-1.50761,-0.87555,2.18930,-0.59328
-15.96330,0.74724,-1.62856,-1.49630,-0.88151,2.18701,-0.58999
-16.00000,0.75303,-1.63410,-1.47822,-0.89104,2.18329,-0.58472
-16.03670,0.75893,-1.64047,-1.45843,-0.90143,2.17950,-0.57938
-16.07339,0.76315,-1.64498,-1.44440,-0.90881,2.17678,-0.57558
-16.11009,0.76969,-1.65339,-1.41992,-0.92159,2.17256,-0.56973
-16.14679,0.77369,-1.65838,-1.40528,-0.92926,2.16996,-0.56617
-16.18349,0.77535,-1.66046,-1.39919,-0.93245,2.16888,-0.56469
-16.22018,0.77603,-1.66129,-1.39674,-0.93373,2.16844,-0.56410
-16.25688,0.77636,-1.66171,-1.39550,-0.93438,2.16822,-0.56380
-16.29358,0.78116,-1.66752,-1.37834,-0.94337,2.16510,-0.55956
-16.33028,0.78492,-1.67214,-1.36478,-0.95049,2.16264,-0.55624
-16.36697,0.80376,-1.69635,-1.29512,-0.98696,2.15026,-0.53981
-16.40367,0.80934,-1.70354,-1.27452,-0.99777,2.14656,-0.53500
-16.44037,0.81366,-1.70890,-1.25901,-1.00593,2.14369,-0.53129
-16.47706,0.81818,-1.71500,-1.24182,-1.01494,2.14068,-0.52742
-16.51376,0.82193,-1.72040,-1.22695,-1.02273,2.13818,-0.52423
-16.55046,0.82461,-1.72441,-1.21601,-1.02845,2.13639,-0.52196
-16.58716,0.82794,-1.72959,-1.20206,-1.03574,2.13417,-0.51915
-16.62385,0.82989,-1.73273,-1.19366,-1.04012,2.13286,-0.51750
-16.66055,0.83381,-1.73919,-1.17654,-1.04905,2.13023,-0.51421
-16.69725,0.83800,-1.74640,-1.15765,-1.05890,2.12742,-0.51070
-16.73394,0.84151,-1.75265,-1.14143,-1.06735,2.12506,-0.50778
-16.77064,0.84467,-1.75847,-1.12644,-1.07515,2.12293,-0.50515
-16.80734,0.84916,-1.76779,-1.10308,-1.08727,2.11990,-0.50145
-16.84404,0.85005,-1.76968,-1.09836,-1.08972,2.11930,-0.50072
-16.88073,0.85260,-1.77564,-1.08380,-1.09726,2.11758,-0.49863
-16.91743,0.85367,-1.77848,-1.07699,-1.10078,2.11686,-0.49776
-16.95413,0.85519,-1.78284,-1.06669,-1.10609,2.11584,-0.49653
-16.99083,0.85639,-1.78659,-1.05794,-1.11059,2.11503,-0.49556
-17.02752,0.85754,-1.79048,-1.04895,-1.11521,2.11425,-0.49463
-17.06422,0.85848,-1.79347,-1.04199,-1.11879,2.11362,-0.49387
-17.10092,0.85872,-1.79422,-1.04025,-1.11969,2.11346,-0.49368
-17.13761,0.85895,-1.79496,-1.03852,-1.12058,2.11330,-0.49349
-17.17431,0.85920,-1.79570,-1.03679,-1.12148,2.11314,-0.49329
-17.21101,0.85944,-1.79644,-1.03505,-1.12237,2.11297,-0.49310
-17.24771,0.86011,-1.79909,-1.02906,-1.12544,2.11252,-0.49256
-17.28440,0.86069,-1.80198,-1.02268,-1.12870,2.11213,-0.49211
-17.32110,0.86087,-1.80306,-1.02033,-1.12990,2.11201,-0.49197
-17.35780,0.86103,-1.80421,-1.01785,-1.13116,2.11190,-0.49184
-17.39450,0.86117,-1.80540,-1.01534,-1.13243,2.11181,-0.49174
-17.43119,0.86129,-1.80665,-1.01271,-1.13377,2.11173,-0.49165
-17.46789,0.86135,-1.80889,-1.00816,-1.13606,2.11170,-0.49163
-17.50459,0.86107,-1.81204,-1.00216,-1.13905,2.11190,-0.49189
-17.54128,0.86057,-1.81522,-0.99633,-1.14193,2.11225,-0.49234
-17.57798,0.86013,-1.81777,-0.99168,-1.14422,2.11257,-0.49274
-17.61468,0.85937,-1.82081,-0.98637,-1.14682,2.11309,-0.49340
-17.65138,0.85781,-1.82597,-0.97768,-1.15105,2.11418,-0.49476
-17.68807,0.85634,-1.83028,-0.97059,-1.15449,2.11521,-0.49604
-17.72477,0.85378,-1.83686,-0.96009,-1.15955,2.11698,-0.49825
-17.76147,0.85244,-1.84018,-0.95486,-1.16206,2.11791,-0.49942
-17.79817,0.85199,-1.84127,-0.95314,-1.16289,2.11822,-0.49981
-17.83486,0.85151,-1.84241,-0.95135,-1.16375,2.11855,-0.50023
-17.87156,0.85105,-1.84350,-0.94966,-1.16456,2.11887,-0.50063
-17.90826,0.85056,-1.84464,-0.94790,-1.16541,2.11921,-0.50105
-17.94495,0.85007,-1.84577,-0.94615,-1.16624,2.11955,-0.50148
-17.98165,0.84967,-1.84662,-0.94486,-1.16686,2.11982,-0.50182
-18.01835,0.84915,-1.84772,-0.94319,-1.16766,2.12018,-0.50227
-18.05505,0.84863,-1.84882,-0.94154,-1.16845,2.12053,-0.50272
-18.09174,0.84811,-1.84992,-0.93989,-1.16923,2.12089,-0.50318
-18.12844,0.84765,-1.85084,-0.93853,-1.16987,2.12121,-0.50358
-18.16514,0.84710,-1.85190,-0.93697,-1.17062,2.12159,-0.50405
-18.20183,0.84602,-1.85363,-0.93466,-1.17169,2.12233,-0.50499
-18.23853,0.84408,-1.85577,-0.93242,-1.17266,2.12366,-0.50665
-18.27523,0.84066,-1.85798,-0.93162,-1.17278,2.12600,-0.50959
-18.31193,0.83730,-1.85896,-0.93325,-1.17168,2.12829,-0.51247
-18.34862,0.83187,-1.85761,-0.94178,-1.16695,2.13197,-0.51711
-18.38532,0.82133,-1.84737,-0.97382,-1.14995,2.13905,-0.52609
-18.42202,0.81594,-1.84049,-0.99360,-1.13952,2.14266,-0.53070
-18.45872,0.81085,-1.83290,-1.01450,-1.12855,2.14605,-0.53505
-18.49541,0.79433,-1.80852,-1.08215,-1.09303,2.15698,-0.54932
-18.53211,0.79052,-1.80280,-1.09803,-1.08469,2.15948,-0.55263
-18.56881,0.78291,-1.79267,-1.12721,-1.06931,2.16447,-0.55930
-18.60550,0.77297,-1.77947,-1.16538,-1.04923,2.17096,-0.56808
-18.64220,0.77088,-1.77669,-1.17343,-1.04498,2.17231,-0.56993
-18.67890,0.76725,-1.77125,-1.18867,-1.03700,2.17467,-0.57316
-18.71560,0.76160,-1.76312,-1.21175,-1.02489,2.17832,-0.57821
-18.75229,0.75677,-1.75580,-1.23224,-1.01417,2.18143,-0.58253
-18.78899,0.75398,-1.75132,-1.24460,-1.00771,2.18322,-0.58505
-18.82569,0.75046,-1.74500,-1.26153,-0.99890,2.18547,-0.58821
-18.86239,0.74251,-1.73348,-1.29436,-0.98173,2.19054,-0.59543
-18.89908,0.73990,-1.72997,-1.30461,-0.97635,2.19220,-0.59781
-18.93578,0.73758,-1.72701,-1.31344,-0.97171,2.19367,-0.59993
-18.97248,0.73489,-1.72371,-1.32342,-0.96646,2.19539,-0.60240
-19.00917,0.73041,-1.71854,-1.33938,-0.95806,2.19822,-0.60653
-19.04587,0.72271,-1.71029,-1.36565,-0.94420,2.20308,-0.61368
-19.08257,0.72024,-1.70776,-1.37389,-0.93985,2.20463,-0.61598
-19.11927,0.71632,-1.70387,-1.38671,-0.93307,2.20709,-0.61965
-19.15596,0.70984,-1.69776,-1.40734,-0.92216,2.21115,-0.62576
-19.19266,0.70512,-1.69352,-1.42198,-0.91439,2.21409,-0.63023
-19.22936,0.70257,-1.69129,-1.42980,-0.91025,2.21568,-0.63265
-19.26606,0.70111,-1.69003,-1.43423,-0.90789,2.21658,-0.63404
-19.30275,0.69935,-1.68832,-1.43998,-0.90486,2.21767,-0.63572
-19.33945,0.69868,-1.68756,-1.44240,-0.90359,2.21809,-0.63636
-19.37615,0.69801,-1.68683,-1.44473,-0.90236,2.21850,-0.63699
-19.41284,0.69670,-1.68525,-1.44960,-0.89981,2.21931,-0.63824
-19.44954,0.68904,-1.67504,-1.48015,-0.88387,2.22401,-0.64557
-19.48624,0.68648,-1.67214,-1.48938,-0.87902,2.22558,-0.64804
-19.52294,0.68223,-1.66853,-1.50235,-0.87215,2.22819,-0.65215
-19.55963,0.67185,-1.66359,-1.52651,-0.85910,2.23453,-0.66231
-19.59633,0.66802,-1.66203,-1.53496,-0.85451,2.23685,-0.66608
-19.63303,0.66480,-1.66115,-1.54124,-0.85106,2.23880,-0.66927
-19.66972,0.66349,-1.66076,-1.54387,-0.84962,2.23960,-0.67057
-19.70642,0.66263,-1.66050,-1.54561,-0.84867,2.24012,-0.67142
-19.74312,0.66219,-1.66036,-1.54652,-0.84818,2.24038,-0.67186
-19.77982,0.66217,-1.66035,-1.54656,-0.84815,2.24040,-0.67188
-19.81651,0.66175,-1.66021,-1.54742,-0.84768,2.24065,-0.67230
-19.85321,0.66089,-1.65992,-1.54923,-0.84670,2.24117,-0.67316
-19.88991,0.66048,-1.65977,-1.55010,-0.84622,2.24142,-0.67357
-19.92661,0.66004,-1.65962,-1.55102,-0.84572,2.24168,-0.67400
-19.96330,0.65961,-1.65947,-1.55194,-0.84522,2.24194,-0.67443
-20.00000,0.65959,-1.65946,-1.55198,-0.84520,2.24195,-0.67445
+0.00000,0.85946,-1.46493,-1.40353,-1.08818,2.11438,-0.49363
+0.11628,0.85941,-1.46494,-1.40354,-1.08817,2.11440,-0.49366
+0.23256,0.85937,-1.46495,-1.40354,-1.08817,2.11443,-0.49370
+0.34884,0.85935,-1.46496,-1.40353,-1.08818,2.11444,-0.49372
+0.46512,0.85934,-1.46496,-1.40351,-1.08818,2.11444,-0.49372
+0.58140,0.85935,-1.46497,-1.40349,-1.08820,2.11444,-0.49371
+0.69767,0.85937,-1.46496,-1.40347,-1.08821,2.11442,-0.49369
+0.81395,0.86007,-1.46479,-1.40289,-1.08853,2.11388,-0.49305
+0.93023,0.86253,-1.46416,-1.40106,-1.08955,2.11200,-0.49084
+1.04651,0.86472,-1.46353,-1.39968,-1.09034,2.11036,-0.48889
+1.16279,0.86665,-1.46287,-1.39874,-1.09091,2.10893,-0.48719
+1.27907,0.86954,-1.46191,-1.39740,-1.09172,2.10680,-0.48467
+1.39535,0.87300,-1.46058,-1.39627,-1.09247,2.10429,-0.48169
+1.51163,0.87706,-1.45897,-1.39518,-1.09322,2.10135,-0.47821
+1.62791,0.88047,-1.45741,-1.39473,-1.09365,2.09890,-0.47532
+1.74419,0.88480,-1.45544,-1.39425,-1.09414,2.09580,-0.47167
+1.86047,0.89139,-1.45216,-1.39420,-1.09459,2.09111,-0.46616
+1.97674,0.89821,-1.44860,-1.39466,-1.09482,2.08627,-0.46051
+2.09302,0.90640,-1.44403,-1.39588,-1.09481,2.08046,-0.45379
+2.20930,0.91320,-1.44005,-1.39735,-1.09459,2.07564,-0.44825
+2.32558,0.92006,-1.43588,-1.39919,-1.09423,2.07077,-0.44269
+2.44186,0.92420,-1.43332,-1.40042,-1.09396,2.06782,-0.43935
+2.55814,0.92964,-1.42982,-1.40231,-1.09349,2.06395,-0.43498
+2.67442,0.93516,-1.42618,-1.40443,-1.09294,2.06002,-0.43056
+2.79070,0.93779,-1.42440,-1.40555,-1.09263,2.05814,-0.42846
+2.90698,0.94185,-1.42154,-1.40747,-1.09207,2.05525,-0.42524
+3.02326,0.94589,-1.41860,-1.40956,-1.09144,2.05236,-0.42203
+3.13953,0.94845,-1.41664,-1.41107,-1.09097,2.05053,-0.42001
+3.25581,0.95241,-1.41343,-1.41372,-1.09011,2.04769,-0.41688
+3.37209,0.95629,-1.41004,-1.41675,-1.08908,2.04491,-0.41383
+3.48837,0.96149,-1.40507,-1.42157,-1.08740,2.04118,-0.40974
+3.60465,0.96791,-1.39878,-1.42782,-1.08520,2.03656,-0.40473
+3.72093,0.97188,-1.39461,-1.43219,-1.08364,2.03370,-0.40163
+3.83721,0.97691,-1.38927,-1.43783,-1.08163,2.03007,-0.39772
+3.95349,0.98075,-1.38511,-1.44229,-1.08003,2.02729,-0.39474
+4.06977,0.98456,-1.38091,-1.44684,-1.07840,2.02453,-0.39179
+4.18605,0.98591,-1.37941,-1.44849,-1.07781,2.02356,-0.39075
+4.30233,0.98852,-1.37640,-1.45184,-1.07660,2.02167,-0.38874
+4.41860,0.99098,-1.37359,-1.45495,-1.07548,2.01988,-0.38685
+4.53488,0.99341,-1.37084,-1.45800,-1.07439,2.01812,-0.38498
+4.65116,0.99606,-1.36776,-1.46145,-1.07315,2.01619,-0.38295
+4.76744,0.99898,-1.36404,-1.46582,-1.07155,2.01407,-0.38071
+4.88372,1.00043,-1.36211,-1.46813,-1.07070,2.01302,-0.37960
+5.00000,1.00069,-1.36175,-1.46858,-1.07054,2.01282,-0.37940
+5.11628,1.00376,-1.35680,-1.47493,-1.06815,2.01059,-0.37705
+5.23256,1.00622,-1.35157,-1.48216,-1.06536,2.00879,-0.37516
+5.34884,1.00743,-1.34805,-1.48731,-1.06334,2.00790,-0.37422
+5.46512,1.00808,-1.34556,-1.49109,-1.06184,2.00741,-0.37371
+5.58140,1.00869,-1.34168,-1.49724,-1.05937,2.00696,-0.37323
+5.69767,1.00885,-1.34037,-1.49935,-1.05852,2.00684,-0.37310
+5.81395,1.00878,-1.33765,-1.50398,-1.05662,2.00688,-0.37313
+5.93023,1.00781,-1.33405,-1.51069,-1.05381,2.00758,-0.37383
+6.04651,1.00531,-1.32942,-1.52011,-1.04978,2.00937,-0.37568
+6.16279,1.00268,-1.32677,-1.52630,-1.04706,2.01127,-0.37764
+6.27907,0.99803,-1.32472,-1.53284,-1.04404,2.01463,-0.38114
+6.39535,0.99165,-1.32358,-1.53902,-1.04104,2.01923,-0.38597
+6.51163,0.97010,-1.32759,-1.54706,-1.03614,2.03474,-0.40249
+6.62791,0.96078,-1.33028,-1.54906,-1.03465,2.04142,-0.40972
+6.74419,0.94887,-1.33520,-1.54929,-1.03370,2.04993,-0.41904
+6.86047,0.93945,-1.33963,-1.54863,-1.03332,2.05663,-0.42647
+6.97674,0.92389,-1.34795,-1.54608,-1.03331,2.06767,-0.43887
+7.09302,0.91465,-1.35323,-1.54410,-1.03352,2.07419,-0.44631
+7.20930,0.90691,-1.35776,-1.54231,-1.03377,2.07963,-0.45257
+7.32558,0.90221,-1.36054,-1.54121,-1.03394,2.08293,-0.45640
+7.44186,0.89759,-1.36329,-1.54012,-1.03411,2.08617,-0.46017
+7.55814,0.89265,-1.36614,-1.53913,-1.03422,2.08962,-0.46423
+7.67442,0.87846,-1.37316,-1.53837,-1.03374,2.09950,-0.47594
+7.79070,0.87118,-1.37652,-1.53846,-1.03331,2.10454,-0.48200
+7.90698,0.86391,-1.37969,-1.53893,-1.03275,2.10957,-0.48809
+8.02326,0.85690,-1.38217,-1.54040,-1.03180,2.11439,-0.49399
+8.13953,0.85078,-1.38417,-1.54200,-1.03086,2.11859,-0.49916
+8.25581,0.84478,-1.38602,-1.54380,-1.02984,2.12270,-0.50427
+8.37209,0.84073,-1.38723,-1.54510,-1.02912,2.12546,-0.50772
+8.48837,0.83876,-1.38781,-1.54575,-1.02876,2.12681,-0.50941
+8.60465,0.83767,-1.38808,-1.54620,-1.02853,2.12754,-0.51034
+8.72093,0.83716,-1.38820,-1.54642,-1.02842,2.12789,-0.51078
+8.83721,0.83661,-1.38831,-1.54669,-1.02829,2.12827,-0.51125
+8.95349,0.83602,-1.38842,-1.54699,-1.02814,2.12867,-0.51175
+9.06977,0.83548,-1.38851,-1.54729,-1.02799,2.12903,-0.51221
+9.18605,0.83491,-1.38859,-1.54763,-1.02783,2.12942,-0.51271
+9.30233,0.83434,-1.38866,-1.54799,-1.02766,2.12981,-0.51320
+9.41860,0.83430,-1.38867,-1.54801,-1.02765,2.12984,-0.51323
+9.53488,0.83426,-1.38867,-1.54804,-1.02763,2.12987,-0.51327
+9.65116,0.83413,-1.38869,-1.54810,-1.02760,2.12995,-0.51337
+9.76744,0.83118,-1.39037,-1.54776,-1.02760,2.13196,-0.51592
+9.88372,0.82780,-1.39313,-1.54599,-1.02815,2.13425,-0.51884
+10.00000,0.82245,-1.39880,-1.54102,-1.02991,2.13788,-0.52350
+10.11628,0.81409,-1.41035,-1.52877,-1.03451,2.14355,-0.53085
+10.23256,0.81003,-1.41701,-1.52105,-1.03750,2.14629,-0.53443
+10.34884,0.80468,-1.42703,-1.50877,-1.04232,2.14990,-0.53920
+10.46512,0.80130,-1.43428,-1.49945,-1.04603,2.15218,-0.54223
+10.58140,0.79687,-1.44412,-1.48663,-1.05118,2.15516,-0.54621
+10.69767,0.79257,-1.45403,-1.47355,-1.05647,2.15805,-0.55010
+10.81395,0.78840,-1.46355,-1.46099,-1.06158,2.16084,-0.55388
+10.93023,0.78493,-1.47143,-1.45059,-1.06583,2.16316,-0.55704
+11.04651,0.78217,-1.47764,-1.44242,-1.06919,2.16500,-0.55956
+11.16279,0.78003,-1.48260,-1.43582,-1.07191,2.16642,-0.56152
+11.27907,0.77591,-1.49297,-1.42170,-1.07777,2.16916,-0.56530
+11.39535,0.77267,-1.50210,-1.40886,-1.08315,2.17132,-0.56830
+11.51163,0.76914,-1.51297,-1.39326,-1.08972,2.17367,-0.57159
+11.62791,0.76691,-1.52007,-1.38296,-1.09409,2.17514,-0.57366
+11.74419,0.76310,-1.53136,-1.36682,-1.10095,2.17767,-0.57723
+11.86047,0.75804,-1.54696,-1.34424,-1.11060,2.18101,-0.58198
+11.97674,0.75586,-1.55373,-1.33441,-1.11484,2.18244,-0.58403
+12.09302,0.75312,-1.56265,-1.32129,-1.12051,2.18425,-0.58663
+12.20930,0.75097,-1.57037,-1.30969,-1.12555,2.18566,-0.58867
+12.32558,0.74898,-1.57865,-1.29694,-1.13111,2.18697,-0.59058
+12.44186,0.74722,-1.58643,-1.28481,-1.13643,2.18813,-0.59227
+12.55814,0.74625,-1.59247,-1.27496,-1.14078,2.18877,-0.59322
+12.67442,0.74539,-1.60077,-1.26095,-1.14699,2.18935,-0.59410
+12.79070,0.74515,-1.60415,-1.25512,-1.14958,2.18952,-0.59435
+12.90698,0.74511,-1.60480,-1.25400,-1.15009,2.18955,-0.59439
+13.02326,0.74508,-1.60539,-1.25296,-1.15055,2.18956,-0.59442
+13.13953,0.74507,-1.60603,-1.25183,-1.15106,2.18957,-0.59444
+13.25581,0.74506,-1.60671,-1.25062,-1.15160,2.18958,-0.59446
+13.37209,0.74509,-1.60796,-1.24833,-1.15262,2.18957,-0.59444
+13.48837,0.74514,-1.60860,-1.24712,-1.15317,2.18953,-0.59440
+13.60465,0.74520,-1.60887,-1.24658,-1.15341,2.18949,-0.59434
+13.72093,0.74522,-1.60890,-1.24651,-1.15344,2.18949,-0.59433
+13.83721,0.74609,-1.60978,-1.24402,-1.15459,2.18893,-0.59353
+13.95349,0.75016,-1.61035,-1.23885,-1.15706,2.18630,-0.58980
+14.06977,0.75560,-1.60905,-1.23567,-1.15869,2.18277,-0.58479
+14.18605,0.76401,-1.60453,-1.23538,-1.15914,2.17728,-0.57708
+14.30233,0.76798,-1.60198,-1.23605,-1.15899,2.17468,-0.57345
+14.41860,0.77275,-1.59821,-1.23814,-1.15823,2.17155,-0.56911
+14.53488,0.77970,-1.59170,-1.24310,-1.15628,2.16696,-0.56280
+14.65116,0.78657,-1.58434,-1.24974,-1.15358,2.16240,-0.55659
+14.76744,0.79160,-1.57872,-1.25503,-1.15141,2.15905,-0.55207
+14.88372,0.79665,-1.57294,-1.26066,-1.14909,2.15568,-0.54754
+15.00000,0.80069,-1.56824,-1.26531,-1.14717,2.15298,-0.54393
+15.11628,0.80373,-1.56468,-1.26887,-1.14571,2.15093,-0.54122
+15.23256,0.80470,-1.56350,-1.27009,-1.14520,2.15029,-0.54036
+15.34884,0.80669,-1.56107,-1.27260,-1.14416,2.14895,-0.53859
+15.46512,0.80865,-1.55858,-1.27525,-1.14306,2.14763,-0.53685
+15.58140,0.81062,-1.55600,-1.27806,-1.14189,2.14630,-0.53511
+15.69767,0.81160,-1.55473,-1.27944,-1.14132,2.14564,-0.53425
+15.81395,0.81355,-1.55221,-1.28216,-1.14019,2.14433,-0.53252
+15.93023,0.81455,-1.55088,-1.28364,-1.13958,2.14365,-0.53164
+16.04651,0.81647,-1.54823,-1.28661,-1.13834,2.14235,-0.52994
+16.16279,0.81841,-1.54552,-1.28971,-1.13705,2.14104,-0.52823
+16.27907,0.82128,-1.54145,-1.29437,-1.13511,2.13909,-0.52571
+16.39535,0.82320,-1.53870,-1.29755,-1.13378,2.13779,-0.52403
+16.51163,0.82605,-1.53443,-1.30260,-1.13168,2.13586,-0.52153
+16.62791,0.82881,-1.53032,-1.30745,-1.12966,2.13397,-0.51911
+16.74419,0.83072,-1.52747,-1.31084,-1.12825,2.13268,-0.51745
+16.86047,0.83258,-1.52466,-1.31419,-1.12686,2.13141,-0.51582
+16.97674,0.83261,-1.52462,-1.31424,-1.12684,2.13139,-0.51580
+17.09302,0.83350,-1.52331,-1.31578,-1.12620,2.13078,-0.51503
+17.20930,0.83439,-1.52200,-1.31731,-1.12557,2.13017,-0.51425
+17.32558,0.83533,-1.52066,-1.31887,-1.12492,2.12953,-0.51343
+17.44186,0.83635,-1.51917,-1.32063,-1.12419,2.12884,-0.51255
+17.55814,0.83732,-1.51770,-1.32238,-1.12347,2.12817,-0.51171
+17.67442,0.83743,-1.51752,-1.32259,-1.12338,2.12809,-0.51161
+17.79070,0.83874,-1.51514,-1.32567,-1.12209,2.12720,-0.51048
+17.90698,0.83905,-1.51453,-1.32650,-1.12175,2.12699,-0.51021
+18.02326,0.84002,-1.51246,-1.32929,-1.12057,2.12632,-0.50936
+18.13953,0.84052,-1.51125,-1.33101,-1.11984,2.12598,-0.50892
+18.25581,0.84101,-1.50983,-1.33309,-1.11895,2.12564,-0.50850
+18.37209,0.84145,-1.50827,-1.33546,-1.11794,2.12533,-0.50810
+18.48837,0.84173,-1.50717,-1.33717,-1.11721,2.12514,-0.50786
+18.60465,0.84198,-1.50598,-1.33905,-1.11640,2.12497,-0.50764
+18.72093,0.84218,-1.50481,-1.34094,-1.11559,2.12483,-0.50745
+18.83721,0.84241,-1.50294,-1.34404,-1.11425,2.12466,-0.50724
+18.95349,0.84253,-1.50166,-1.34619,-1.11332,2.12458,-0.50713
+19.06977,0.84257,-1.50104,-1.34725,-1.11286,2.12455,-0.50709
+19.18605,0.84261,-1.50038,-1.34838,-1.11237,2.12452,-0.50705
+19.30233,0.84264,-1.49972,-1.34952,-1.11188,2.12450,-0.50702
+19.41860,0.84265,-1.49907,-1.35065,-1.11139,2.12449,-0.50700
+19.53488,0.84266,-1.49842,-1.35178,-1.11090,2.12448,-0.50700
+19.65116,0.84265,-1.49778,-1.35291,-1.11040,2.12448,-0.50699
+19.76744,0.84263,-1.49715,-1.35404,-1.10991,2.12449,-0.50700
+19.88372,0.84261,-1.49648,-1.35525,-1.10939,2.12451,-0.50702
+20.00000,0.84261,-1.49644,-1.35532,-1.10936,2.12451,-0.50702
diff --git a/python/examples/path_in_pixels.csv b/python/examples/path_in_pixels.csv
index 24c30ee1551a9d021b23617b4c930d39999fff32..8fb43e1a02c2ae150155d44c6aabe129fe19716a 100644
--- a/python/examples/path_in_pixels.csv
+++ b/python/examples/path_in_pixels.csv
@@ -1,749 +1,173 @@
-0.12463,0.50316
-0.13500,0.50316
-0.14981,0.50183
-0.16759,0.50050
-0.19573,0.49784
-0.21499,0.49517
-0.22981,0.49384
-0.25499,0.49118
-0.27425,0.48851
-0.29054,0.48585
-0.30388,0.48452
-0.32314,0.48318
-0.34091,0.48185
-0.35721,0.48052
-0.37350,0.47919
-0.40165,0.47786
-0.42091,0.47652
-0.43721,0.47519
-0.46091,0.47253
-0.51128,0.46853
-0.53498,0.46853
-0.56313,0.46853
-0.57646,0.46720
-0.58535,0.46720
-0.59720,0.46720
-0.60164,0.46720
-0.60757,0.46720
-0.60905,0.46720
-0.62238,0.46986
-0.65201,0.47253
-0.66090,0.47253
-0.68757,0.47253
-0.70386,0.47253
-0.72016,0.47253
-0.72905,0.47253
-0.74090,0.47253
-0.75275,0.47253
-0.76312,0.47253
-0.77497,0.47253
-0.78386,0.47253
-0.78979,0.47120
-0.79571,0.46986
-0.80460,0.46986
-0.81201,0.46853
-0.81941,0.46853
-0.82978,0.46853
-0.83719,0.46853
-0.84460,0.46853
-0.84904,0.46853
-0.85497,0.46853
-0.85793,0.46853
-0.85941,0.46853
-0.86089,0.46853
-0.86238,0.46853
-0.86238,0.46853
-0.86386,0.46853
-0.86386,0.46853
-0.86534,0.46853
-0.86978,0.46853
-0.87126,0.46853
-0.87275,0.46853
-0.87275,0.46853
-0.87423,0.46853
-0.87719,0.46853
-0.87867,0.46853
-0.87867,0.46853
-0.86534,0.47786
-0.84164,0.49251
-0.83127,0.49917
-0.81941,0.50849
-0.79423,0.52181
-0.77793,0.52980
-0.76312,0.53646
-0.75423,0.54312
-0.73497,0.55778
-0.71720,0.56843
-0.68905,0.58575
-0.67572,0.59241
-0.65942,0.60306
-0.64461,0.61106
-0.62831,0.61772
-0.61646,0.62438
-0.60016,0.63503
-0.55572,0.65501
-0.54091,0.66034
-0.51276,0.66833
-0.49498,0.67233
-0.48017,0.67766
-0.46239,0.68565
-0.43424,0.69897
-0.41943,0.70563
-0.39573,0.72028
-0.38536,0.72827
-0.37647,0.73360
-0.37054,0.73893
-0.36462,0.74292
-0.36165,0.74426
-0.35869,0.74692
-0.34091,0.75758
-0.32462,0.76424
-0.31277,0.77090
-0.29795,0.77889
-0.29203,0.78288
-0.28462,0.78688
-0.28017,0.79088
-0.27277,0.79487
-0.26684,0.80020
-0.26092,0.80420
-0.25351,0.80819
-0.24018,0.81618
-0.23721,0.81885
-0.23425,0.82018
-0.23129,0.82151
-0.22981,0.82284
-0.22684,0.82418
-0.22240,0.82684
-0.21795,0.82817
-0.21647,0.82817
-0.21499,0.82817
-0.21351,0.82817
-0.21055,0.82817
-0.20758,0.82817
-0.20610,0.82950
-0.20314,0.82950
-0.20166,0.83084
-0.20166,0.83084
-0.20018,0.83084
-0.19870,0.83084
-0.19721,0.83084
-0.20018,0.82418
-0.20610,0.81086
-0.20907,0.79887
-0.21351,0.79221
-0.21647,0.78555
-0.22092,0.77756
-0.22981,0.76290
-0.23721,0.74958
-0.24314,0.73626
-0.25351,0.71628
-0.26388,0.69897
-0.26980,0.68432
-0.27573,0.67233
-0.27869,0.66300
-0.28166,0.65634
-0.28314,0.64968
-0.29203,0.63370
-0.30091,0.61106
-0.30832,0.59507
-0.31425,0.58442
-0.32314,0.56843
-0.33351,0.54712
-0.33943,0.53646
-0.34388,0.52581
-0.34684,0.51648
-0.34980,0.50183
-0.35869,0.47519
-0.36462,0.45255
-0.37054,0.43390
-0.39128,0.37662
-0.39721,0.36064
-0.40610,0.33933
-0.42091,0.30070
-0.42387,0.28738
-0.42684,0.27140
-0.42684,0.25674
-0.42684,0.24476
-0.42684,0.22877
-0.42980,0.20879
-0.42980,0.19680
-0.43128,0.18615
-0.43276,0.18082
-0.43424,0.17150
-0.43721,0.16217
-0.43869,0.15551
-0.43869,0.15152
-0.43869,0.14752
-0.43869,0.14352
-0.43869,0.13953
-0.43869,0.13686
-0.43869,0.13287
-0.43869,0.13020
-0.43869,0.12754
-0.43869,0.12488
-0.43869,0.12221
-0.44017,0.11955
-0.44017,0.11688
-0.44017,0.11156
-0.44017,0.10889
-0.44461,0.11822
-0.45646,0.14086
-0.46387,0.15551
-0.47128,0.16750
-0.48017,0.17949
-0.48609,0.19014
-0.49646,0.20613
-0.51276,0.22611
-0.52609,0.24209
-0.54239,0.26340
-0.55424,0.28072
-0.57498,0.32201
-0.58387,0.33800
-0.59868,0.36330
-0.60905,0.37929
-0.62238,0.40060
-0.62979,0.41525
-0.64016,0.43257
-0.64609,0.44456
-0.65053,0.45255
-0.65646,0.46187
-0.65942,0.46720
-0.66090,0.46853
-0.66238,0.47120
-0.66386,0.47253
-0.67275,0.50450
-0.67868,0.52448
-0.68312,0.54046
-0.68905,0.55511
-0.69349,0.56710
-0.69646,0.58042
-0.70090,0.59241
-0.70979,0.61505
-0.71423,0.62837
-0.72016,0.64169
-0.72608,0.65501
-0.73053,0.66434
-0.73497,0.67366
-0.73794,0.68165
-0.74979,0.69764
-0.75423,0.70163
-0.76016,0.70962
-0.76460,0.71495
-0.77201,0.72428
-0.77793,0.73227
-0.78238,0.74159
-0.79275,0.75758
-0.80016,0.76557
-0.80608,0.77489
-0.81349,0.78155
-0.81793,0.78821
-0.82534,0.79487
-0.83127,0.80286
-0.83423,0.80686
-0.83867,0.81352
-0.84015,0.81618
-0.84312,0.82018
-0.84608,0.82551
-0.84904,0.83084
-0.85201,0.83483
-0.85349,0.84016
-0.85497,0.84416
-0.85645,0.84682
-0.85793,0.84815
-0.85793,0.84948
-0.85793,0.85082
-0.85497,0.84948
-0.84756,0.84682
-0.83571,0.84416
-0.82682,0.84149
-0.81645,0.83750
-0.80904,0.83483
-0.79423,0.82950
-0.77942,0.82284
-0.76756,0.81752
-0.75275,0.81219
-0.74534,0.80819
-0.73645,0.80420
-0.72608,0.79887
-0.71275,0.79354
-0.70238,0.78954
-0.69053,0.78555
-0.68016,0.78288
-0.67275,0.78022
-0.66386,0.77756
-0.65201,0.77356
-0.63424,0.76956
-0.62238,0.76557
-0.61053,0.76024
-0.58831,0.75225
-0.57202,0.74559
-0.55720,0.74159
-0.54535,0.73760
-0.53498,0.73493
-0.52461,0.73094
-0.50980,0.72561
-0.49350,0.71895
-0.48165,0.71495
-0.46832,0.71096
-0.45943,0.70829
-0.43276,0.69630
-0.41647,0.68698
-0.40461,0.67899
-0.39424,0.67233
-0.38387,0.66434
-0.37202,0.65634
-0.35869,0.64835
-0.34536,0.64302
-0.33351,0.63770
-0.32462,0.63503
-0.31425,0.62837
-0.30684,0.62438
-0.29943,0.61905
-0.28758,0.61239
-0.27869,0.60573
-0.26832,0.59774
-0.25795,0.59241
-0.25203,0.58708
-0.24462,0.58442
-0.24018,0.58175
-0.23573,0.57909
-0.23129,0.57642
-0.22684,0.57376
-0.22240,0.57110
-0.21795,0.56843
-0.20462,0.56177
-0.19721,0.55778
-0.19425,0.55511
-0.18833,0.55245
-0.18388,0.54845
-0.17796,0.54446
-0.17499,0.54179
-0.17055,0.53780
-0.16907,0.53513
-0.16610,0.53247
-0.16314,0.53114
-0.16166,0.52847
-0.15870,0.52581
-0.15722,0.52314
-0.15277,0.52048
-0.14981,0.51782
-0.14536,0.51515
-0.14240,0.51382
-0.13944,0.51249
-0.13796,0.51116
-0.13648,0.51116
-0.13500,0.50982
-0.13351,0.50849
-0.13055,0.50716
-0.12759,0.50583
-0.12463,0.50316
-0.12166,0.50316
-0.11870,0.50316
-0.11870,0.50316
-0.11722,0.50316
-0.11574,0.50316
-0.11426,0.50316
-0.12314,0.48452
-0.13055,0.46853
-0.14092,0.44722
-0.14685,0.43656
-0.15425,0.42324
-0.16166,0.40992
-0.16610,0.39927
-0.17203,0.38994
-0.17499,0.38195
-0.18536,0.35664
-0.19129,0.34066
-0.19277,0.32867
-0.19573,0.31802
-0.19721,0.31269
-0.19870,0.30736
-0.20166,0.29937
-0.20314,0.29404
-0.20907,0.28472
-0.21351,0.27539
-0.22092,0.26474
-0.22536,0.25808
-0.22981,0.25142
-0.23277,0.24742
-0.23573,0.24476
-0.23721,0.24076
-0.24166,0.23410
-0.24610,0.22877
-0.25055,0.22078
-0.25499,0.21412
-0.25795,0.20613
-0.26092,0.20080
-0.26536,0.19414
-0.26832,0.18881
-0.27129,0.18348
-0.27425,0.17949
-0.27869,0.17416
-0.28462,0.16883
-0.29054,0.16350
-0.29795,0.15951
-0.30388,0.15418
-0.30832,0.15018
-0.31425,0.14486
-0.31721,0.14086
-0.32017,0.13820
-0.32165,0.13553
-0.32610,0.13154
-0.33054,0.12754
-0.33647,0.12221
-0.34980,0.11422
-0.35721,0.11156
-0.36313,0.10889
-0.36906,0.10623
-0.37350,0.10490
-0.37943,0.10356
-0.39128,0.10090
-0.39721,0.10090
-0.40610,0.09957
-0.41202,0.09824
-0.42684,0.09690
-0.43128,0.09690
-0.44609,0.09424
-0.45202,0.09424
-0.46239,0.09424
-0.47128,0.09291
-0.48313,0.09291
-0.49054,0.09291
-0.50091,0.09291
-0.50980,0.09291
-0.51720,0.09424
-0.52313,0.09557
-0.52905,0.09557
-0.53498,0.09690
-0.53942,0.09824
-0.54239,0.09957
-0.54535,0.10090
-0.54683,0.10090
-0.56313,0.11156
-0.57202,0.11688
-0.58535,0.12621
-0.59868,0.13553
-0.61498,0.14619
-0.62831,0.15551
-0.64016,0.16484
-0.64609,0.17016
-0.65349,0.17549
-0.66238,0.18348
-0.67720,0.19414
-0.68460,0.19947
-0.69201,0.20613
-0.70090,0.21279
-0.70831,0.22078
-0.71868,0.23010
-0.72460,0.23943
-0.74534,0.25941
-0.75571,0.27140
-0.76608,0.28338
-0.77645,0.29271
-0.78238,0.30070
-0.78979,0.30736
-0.79423,0.31402
-0.80460,0.32734
-0.80904,0.33533
-0.81349,0.34332
-0.81793,0.35132
-0.82238,0.35931
-0.82978,0.36996
-0.83571,0.37662
-0.85201,0.39527
-0.85793,0.40326
-0.86238,0.40992
-0.86386,0.41259
-0.86682,0.41658
-0.86830,0.41792
-0.86830,0.41925
-0.87423,0.43257
-0.87719,0.44056
-0.87867,0.44456
-0.88015,0.44855
-0.88163,0.45255
-0.88312,0.45788
-0.88608,0.46587
-0.88756,0.47120
-0.88904,0.47519
-0.89052,0.48185
-0.89052,0.48718
-0.89052,0.49251
-0.89052,0.50050
-0.89200,0.50450
-0.89200,0.50716
-0.89349,0.51249
-0.89497,0.51782
-0.89497,0.52181
-0.89645,0.52714
-0.89645,0.53380
-0.89793,0.53913
-0.89793,0.54712
-0.89793,0.55378
-0.89793,0.56044
-0.89793,0.56710
-0.89941,0.57376
-0.89941,0.57909
-0.89941,0.59108
-0.89941,0.59374
-0.89941,0.59640
-0.89941,0.59774
-0.89941,0.59907
-0.89941,0.60040
-0.89941,0.60173
-0.89645,0.61239
-0.89497,0.62304
-0.89349,0.63503
-0.89349,0.64169
-0.89349,0.64702
-0.89349,0.65235
-0.89349,0.65768
-0.89349,0.66700
-0.89349,0.67233
-0.89200,0.67766
-0.89200,0.68165
-0.89200,0.68698
-0.89200,0.68964
-0.89200,0.69364
-0.89052,0.70030
-0.88904,0.70563
-0.88756,0.71096
-0.88756,0.71229
-0.88756,0.71762
-0.88756,0.72294
-0.88608,0.72561
-0.88608,0.72694
-0.88608,0.72960
-0.88608,0.73360
-0.88608,0.73760
-0.88608,0.74159
-0.88608,0.74292
-0.88460,0.74692
-0.88460,0.74958
-0.88460,0.75624
-0.88460,0.75891
-0.88460,0.76157
-0.88460,0.76424
-0.88460,0.76690
-0.88312,0.76956
-0.88163,0.77356
-0.88015,0.77622
-0.88015,0.77889
-0.87867,0.78155
-0.87867,0.78288
-0.87867,0.78422
-0.87867,0.78555
-0.87719,0.78688
-0.87719,0.78821
-0.87571,0.79088
-0.87571,0.79221
-0.87423,0.79487
-0.87275,0.79754
-0.87126,0.80020
-0.86978,0.80153
-0.86830,0.80420
-0.86534,0.80686
-0.86386,0.80952
-0.86386,0.81086
-0.86238,0.81219
-0.86238,0.81352
-0.86089,0.81618
-0.85941,0.81752
-0.85941,0.81885
-0.85793,0.82018
-0.85793,0.82151
-0.85645,0.82284
-0.85497,0.82684
-0.85349,0.82817
-0.85201,0.83217
-0.85052,0.83483
-0.84904,0.83750
-0.84756,0.84016
-0.84608,0.84149
-0.84460,0.84416
-0.84312,0.84549
-0.84015,0.84948
-0.83867,0.85082
-0.83719,0.85348
-0.83719,0.85614
-0.83571,0.85748
-0.83423,0.86014
-0.83275,0.86147
-0.83127,0.86280
-0.82978,0.86414
-0.82830,0.86680
-0.82534,0.86813
-0.82090,0.87213
-0.81645,0.87479
-0.81053,0.87879
-0.80312,0.88278
-0.79275,0.88545
-0.78830,0.88678
-0.78386,0.88811
-0.77942,0.88944
-0.77497,0.89078
-0.77053,0.89211
-0.76756,0.89344
-0.76016,0.89477
-0.75127,0.89744
-0.73794,0.90010
-0.72757,0.90143
-0.71275,0.90543
-0.70386,0.90676
-0.68905,0.90809
-0.66979,0.91209
-0.65201,0.91475
-0.63868,0.91608
-0.62831,0.91742
-0.61350,0.91875
-0.59868,0.92008
-0.58979,0.92008
-0.58387,0.92008
-0.57350,0.91875
-0.54979,0.92008
-0.54387,0.92008
-0.53054,0.92141
-0.51572,0.92274
-0.50387,0.92408
-0.49202,0.92541
-0.48017,0.92541
-0.47424,0.92541
-0.46683,0.92541
-0.46091,0.92541
-0.45350,0.92541
-0.44461,0.92541
-0.43572,0.92541
-0.42684,0.92541
-0.41795,0.92541
-0.41054,0.92541
-0.40461,0.92541
-0.39721,0.92541
-0.38980,0.92541
-0.38239,0.92541
-0.37647,0.92541
-0.37202,0.92541
-0.36758,0.92541
-0.36313,0.92541
-0.35869,0.92541
-0.35425,0.92541
-0.34684,0.92541
-0.33943,0.92541
-0.33647,0.92541
-0.33202,0.92408
-0.32758,0.92274
-0.32462,0.92141
-0.32017,0.92141
-0.31277,0.92008
-0.30832,0.91875
-0.30240,0.91742
-0.29795,0.91608
-0.29351,0.91475
-0.28906,0.91209
-0.28462,0.90942
-0.27425,0.90410
-0.26980,0.90276
-0.26092,0.89610
-0.25795,0.89211
-0.25351,0.88811
-0.25055,0.88412
-0.24610,0.88145
-0.24166,0.87746
-0.23869,0.87346
-0.23277,0.86680
-0.23129,0.86547
-0.22832,0.86414
-0.22536,0.86280
-0.22240,0.86147
-0.21795,0.85881
-0.21499,0.85614
-0.21203,0.85481
-0.20907,0.85348
-0.20610,0.85348
-0.20462,0.85215
-0.20018,0.84948
-0.19721,0.84682
-0.19425,0.84416
-0.18981,0.84016
-0.18240,0.83084
-0.17944,0.82684
-0.17647,0.82284
-0.17499,0.82018
-0.17203,0.81752
-0.16907,0.81352
-0.16610,0.80952
-0.16314,0.80686
-0.16166,0.80420
-0.15870,0.80153
-0.15573,0.79887
-0.15277,0.79487
-0.14833,0.78954
-0.14388,0.78288
-0.14092,0.77489
-0.13648,0.76690
-0.13351,0.76024
-0.13055,0.75491
-0.12759,0.74825
-0.12463,0.74159
-0.12314,0.73760
-0.12166,0.73360
-0.12018,0.72827
-0.11722,0.72294
-0.11426,0.71895
-0.11129,0.71229
-0.10981,0.70696
-0.10685,0.70030
-0.10537,0.69364
-0.10240,0.68565
-0.10240,0.67899
-0.10092,0.67233
-0.09944,0.66700
-0.09796,0.66434
-0.09648,0.65768
-0.09500,0.65102
-0.09352,0.64436
-0.09203,0.63770
-0.08907,0.62704
-0.08907,0.62304
-0.08907,0.61905
-0.08907,0.61239
-0.08759,0.60573
-0.08611,0.60040
-0.08611,0.59640
-0.08463,0.58708
-0.08463,0.57909
-0.08315,0.57376
-0.08315,0.56843
-0.08315,0.55778
-0.08315,0.55511
-0.08315,0.54446
-0.08315,0.54046
-0.08315,0.53646
-0.08463,0.53247
-0.08463,0.52847
-0.08611,0.52581
-0.08759,0.52181
-0.08907,0.51515
-0.09055,0.51116
-0.09203,0.50716
-0.09352,0.50050
-0.09500,0.49784
-0.09500,0.49517
-0.09500,0.49118
-0.09648,0.48718
-0.09796,0.48318
-0.09944,0.48052
-0.10092,0.47919
-0.10240,0.47652
-0.10240,0.47519
-0.10240,0.47386
-0.10240,0.47253
-0.10240,0.47120
-0.10240,0.46986
-0.10537,0.46720
-0.10833,0.46587
-0.10981,0.46320
-0.11129,0.46187
-0.11129,0.46054
-0.11277,0.46054
-0.11277,0.45921
-0.11277,0.45921
+0.55466,0.55325
+0.55466,0.55325
+0.55466,0.55440
+0.54454,0.56710
+0.53645,0.57518
+0.52634,0.58442
+0.51623,0.59134
+0.51016,0.59711
+0.50612,0.60058
+0.50207,0.60289
+0.49803,0.60404
+0.49398,0.60750
+0.48792,0.60981
+0.48185,0.61328
+0.47578,0.61443
+0.46971,0.61789
+0.45960,0.62136
+0.44949,0.62482
+0.43735,0.62828
+0.42724,0.63059
+0.41713,0.63290
+0.41106,0.63405
+0.40297,0.63521
+0.39488,0.63636
+0.39084,0.63636
+0.38477,0.63636
+0.37870,0.63636
+0.37466,0.63521
+0.36859,0.63405
+0.36252,0.63175
+0.35443,0.62828
+0.34432,0.62597
+0.33825,0.62251
+0.33016,0.62020
+0.32410,0.61789
+0.31803,0.61558
+0.31601,0.61443
+0.31196,0.61212
+0.30792,0.61097
+0.30387,0.60981
+0.29983,0.60750
+0.29578,0.60289
+0.29376,0.60058
+0.29376,0.59942
+0.28972,0.59250
+0.28769,0.58326
+0.28769,0.57633
+0.28769,0.57172
+0.28769,0.56479
+0.28769,0.56248
+0.28972,0.55671
+0.29376,0.54863
+0.29781,0.53824
+0.30387,0.53131
+0.31399,0.52439
+0.32410,0.51746
+0.35443,0.50476
+0.36657,0.50014
+0.38275,0.49668
+0.39488,0.49322
+0.41511,0.48745
+0.42724,0.48398
+0.43735,0.48052
+0.44342,0.47821
+0.44949,0.47590
+0.45556,0.47244
+0.47174,0.45974
+0.47983,0.45281
+0.48792,0.44589
+0.49398,0.43665
+0.50005,0.42973
+0.50612,0.42280
+0.51016,0.41818
+0.51219,0.41587
+0.51219,0.41356
+0.51219,0.41241
+0.51219,0.41126
+0.51219,0.41010
+0.51219,0.40895
+0.51219,0.40779
+0.51219,0.40664
+0.51219,0.40664
+0.51219,0.40664
+0.51421,0.40779
+0.52634,0.41587
+0.53443,0.42280
+0.54454,0.42973
+0.56072,0.44012
+0.56881,0.44704
+0.58095,0.45512
+0.58904,0.46205
+0.60117,0.46898
+0.61331,0.47590
+0.62544,0.48167
+0.63555,0.48629
+0.64364,0.48975
+0.64971,0.49322
+0.66185,0.50130
+0.67196,0.50938
+0.68409,0.51861
+0.69218,0.52439
+0.70634,0.53131
+0.72454,0.54286
+0.73263,0.54747
+0.74274,0.55440
+0.75083,0.56133
+0.75892,0.56941
+0.76701,0.57633
+0.77106,0.58442
+0.77713,0.59481
+0.77915,0.59942
+0.77915,0.60058
+0.77915,0.60173
+0.77915,0.60289
+0.77915,0.60404
+0.77915,0.60635
+0.77713,0.60866
+0.77308,0.61097
+0.76904,0.61212
+0.75892,0.61558
+0.74274,0.62020
+0.72859,0.62367
+0.71039,0.62713
+0.70230,0.62828
+0.69218,0.62828
+0.67803,0.62828
+0.66387,0.62713
+0.65376,0.62713
+0.64364,0.62713
+0.63555,0.62713
+0.62949,0.62713
+0.62746,0.62597
+0.62342,0.62597
+0.61938,0.62482
+0.61533,0.62367
+0.61331,0.62367
+0.60926,0.62367
+0.60724,0.62251
+0.60320,0.62136
+0.59915,0.62020
+0.59308,0.61905
+0.58904,0.61789
+0.58297,0.61558
+0.57690,0.61443
+0.57286,0.61328
+0.56881,0.61212
+0.56881,0.61212
+0.56679,0.61212
+0.56477,0.61212
+0.56275,0.61212
+0.56072,0.61097
+0.55870,0.60981
+0.55870,0.60866
+0.55668,0.60404
+0.55668,0.60173
+0.55466,0.59942
+0.55466,0.59596
+0.55466,0.59250
+0.55466,0.58903
+0.55466,0.58672
+0.55466,0.58442
+0.55466,0.58211
+0.55466,0.57864
+0.55466,0.57633
+0.55466,0.57518
+0.55466,0.57403
+0.55466,0.57287
+0.55466,0.57172
+0.55466,0.57056
+0.55466,0.56941
+0.55466,0.56825
+0.55466,0.56710
+0.55466,0.56710
diff --git a/python/examples/point_force_control.py b/python/examples/point_force_control.py
new file mode 100644
index 0000000000000000000000000000000000000000..56ff4171721eca9a3cee7f76ddb69f352c954c9a
--- /dev/null
+++ b/python/examples/point_force_control.py
@@ -0,0 +1,9 @@
+"""
+idea
+----
+do nothing until you feel a force of some number of newtons.
+then apply 2N in gripper z direction. and try to maintain it
+if this works try writing (wiping first) with a force reference instead of
+compliance.
+other than the force, maintaing being in whatever z axis is in cartesian space.
+"""
diff --git a/python/ur_simple_control/__init__.py b/python/ur_simple_control/__init__.py
index 5cb8d28a0a0fae94a10ed7441ce04d971473d20a..75986aa4b30ac7e439246be67064f4e90dc56679 100644
--- a/python/ur_simple_control/__init__.py
+++ b/python/ur_simple_control/__init__.py
@@ -1,4 +1,4 @@
from ur_simple_control import clik, dmp, robot_descriptions, util, visualize
__all__ = [
-"clik", "dmp", "robot_descriptions", "util", "visualize", "basics"
+"clik", "dmp", "robot_descriptions", "util", "visualize", "basics", "vision"
]
diff --git a/python/ur_simple_control/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/__pycache__/__init__.cpython-312.pyc
index e85b1e4e44d05747ce2ca03bebd75077c0d0aa51..d2618cf293d8233bf32c84fc0512a79ee7bcca00 100644
Binary files a/python/ur_simple_control/__pycache__/__init__.cpython-312.pyc and b/python/ur_simple_control/__pycache__/__init__.cpython-312.pyc differ
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index 14d9c2bf74a6ca6676bc6f7fa0e0d6feada992d5..f74d281c0b1c18985912c21b5766180e6ed6e926 100644
Binary files a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc differ
diff --git a/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc b/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc
index b1e46579879d5b082e636e725db1b2c84e396044..e1f51ccc304659f0c256d15a324b24fc9c0a6ee0 100644
Binary files a/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc and b/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc differ
diff --git a/python/ur_simple_control/basics/basics.py b/python/ur_simple_control/basics/basics.py
index 17e58a4680408ae132ee4980a8d1be04afc787f5..baf51a28097523467884b89a3f35003d05986a50 100644
--- a/python/ur_simple_control/basics/basics.py
+++ b/python/ur_simple_control/basics/basics.py
@@ -92,7 +92,7 @@ def followKinematicJointTrajPControlLoop(stop_at_final : bool, robot: RobotManag
t_index_upper = len(reference['qs']) - 1
q_ref = reference['qs'][t_index_upper]
if (t_index_upper == len(reference['qs']) - 1) and stop_at_final:
- v_ref = np.zeros(len(reference['vs'][t_index_upper]))
+ v_ref = np.zeros(robot.model.nv)
else:
v_ref = reference['vs'][t_index_upper]
@@ -115,6 +115,7 @@ def followKinematicJointTrajPControlLoop(stop_at_final : bool, robot: RobotManag
q_ref = pin.integrate(robot.model, reference['qs'][t_index_lower], reference['vs'][t_index_lower] * time_difference)
+ # TODO: why not use pin.difference for both?
if robot.robot_name == "ur5e":
error_q = q_ref - q
if robot.robot_name == "heron":
@@ -152,10 +153,8 @@ def followKinematicJointTrajP(args, robot, reference):
}
loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
loop_manager.run()
- # TODO: remove, this isn't doing anything
- #time.sleep(0.01)
if args.debug_prints:
- print("MoveJP done: convergence achieved, reached destionation!")
+ print("followKinematicJointTrajP done: reached path destionation!")
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 1e511a5d35b53a6d8396fac59e850491e60073bc..5ec984d5112bf771de1f6fb5ae47411e9d49a5fb 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -401,6 +401,9 @@ def clikCartesianPathIntoJointPath(args, robot, path, \
# TODO handle saving more consistently/intentionally
# (although this definitely works right now and isn't bad, just mid)
# os.makedir -p a data dir and save there, this is ugly
+ # TODO: check if we actually need this and if not remove the saving
+ # whatever code uses this is responsible to log it if it wants it,
+ # let's not have random files around.
np.savetxt("./joint_trajectory.csv", joint_trajectory, delimiter=',', fmt='%.5f')
return joint_trajectory
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 898d1e58d45506aa29464558598f1832c44198db..28f10d365be04af229c15804e72a1f71e7fb3e3e 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -1,6 +1,8 @@
# PYTHON_ARGCOMPLETE_OK
# TODO rename all private variables to start with '_'
# TODO: make importing nicer with __init__.py files
+# put all managers into a managers folder,
+# and have each manager in a separate file, this is getting ridiculous
import pinocchio as pin
import numpy as np
import time
@@ -22,6 +24,7 @@ import argcomplete, argparse
from sys import exc_info
from types import NoneType
from os import getpid
+from functools import partial
"""
general notes
@@ -223,22 +226,9 @@ class ControlLoopManager:
self.log_dict[key] = []
if self.args.real_time_plotting:
- self.plotter_queue = Queue()
- if self.args.debug_prints:
- print("CONTROL_LOOP_MANAGER", self.controlLoop, ": i created queue for real time plotting:", self.plotter_queue)
- print("CONTROL_LOOP_MANAGER: i am creating and starting the real-time-plotter process")
- self.real_time_plotter_process = Process(target=realTimePlotter,
- args=(self.args, self.plotter_queue, ))
+ self.plotter_manager = ProcessManager(args, realTimePlotter, log_item, 0)
# give real-time plotter some time to set itself up
- self.real_time_plotter_process.start()
- if self.args.debug_prints:
- print("CONTROL_LOOP_MANAGER: real_time_plotter_process started")
- # wait for feedback that the thing has started
- self.plotter_queue.get()
- self.plotter_queue.put(log_item)
- if self.args.debug_prints:
- print("CONTROL_LOOP_MANAGER: i managed to put initializing log_item to queue")
-
+ #self.plotter_manager.sendCommand(log_item)
def run(self):
"""
@@ -285,16 +275,16 @@ class ControlLoopManager:
if i % 20 == 0:
# don't send what wasn't ready
if self.args.visualize_manipulator:
- if self.robot_manager.manipulator_visualizer_queue.qsize() < 5:
- self.robot_manager.updateViz({"q" : self.robot_manager.q,
- "T_w_e" : self.robot_manager.getT_w_e()})
+ self.robot_manager.visualizer_manager.sendCommand({"q" : self.robot_manager.q,
+ "T_w_e" : self.robot_manager.getT_w_e()})
+ #if self.robot_manager.manipulator_visualizer_queue.qsize() < 5:
+ # self.robot_manager.updateViz({"q" : self.robot_manager.q,
+ # "T_w_e" : self.robot_manager.getT_w_e()})
if self.args.real_time_plotting:
# don't put new stuff in if it didn't handle the previous stuff.
# it's a plotter, who cares if it's late.
# the number 5 is arbitrary
- if self.plotter_queue.qsize() < 5:
- self.plotter_queue.put_nowait(log_item)
- #print("plotter_queue size status:", self.plotter_queue.qsize())
+ self.plotter_manager.sendCommand(log_item)
# break if done
if breakFlag:
@@ -318,16 +308,7 @@ class ControlLoopManager:
# for over
######################################################################
if self.args.real_time_plotting:
- if self.args.debug_prints:
- print("i am putting befree in plotter_queue to stop the real time visualizer")
- self.plotter_queue.put_nowait("befree")
- try:
- self.real_time_plotter_process.terminate()
- if self.args.debug_prints:
- print("terminated real_time_plotter_process")
- except AttributeError:
- if self.args.debug_prints:
- print("real-time-plotter is dead already")
+ self.plotter_manager.terminateProcess()
if self.args.save_log:
self.robot_manager.log_manager.storeControlLoopRun(self.log_dict, self.controlLoop.func.__name__, self.final_iteration)
if i < self.max_iterations -1:
@@ -357,6 +338,7 @@ class ControlLoopManager:
for i in range(300):
vel_cmd = np.zeros(self.robot_manager.model.nv)
self.robot_manager.sendQd(vel_cmd)
+
# hopefully this actually stops it
if not self.args.pinocchio_only:
self.robot_manager.rtde_control.speedStop(1)
@@ -371,48 +353,11 @@ class ControlLoopManager:
self.robot_manager.log_manager.storeControlLoopRun(self.log_dict, self.controlLoop.func.__name__, self.final_iteration)
self.robot_manager.log_manager.saveLog()
- ####################################
- # kill real-time-plotter process #
- ####################################
if self.args.real_time_plotting:
- if self.args.debug_prints:
- print("i am putting befree in plotter_queue to stop the real time visualizer")
- self.plotter_queue.put_nowait("befree")
- # for good measure
- time.sleep(0.1)
- #if type(self.real_time_plotter_process) != NoneType:
- try:
- self.real_time_plotter_process.terminate()
- if self.args.debug_prints:
- print("terminated real_time_plotter_process")
- except AttributeError:
- if self.args.debug_prints:
- print("real-time-plotter is dead already")
+ self.plotter_manager.terminateProcess()
- #############################
- # kill visualizer process #
- #############################
if self.args.visualize_manipulator:
- if self.args.debug_prints:
- print("i am putting befree in manipulator to stop the manipulator visualizer")
- self.robot_manager.manipulator_visualizer_queue.put_nowait({"befree" : "befree"})
- # for good measure
- time.sleep(0.1)
- #if type(self.robot_manager.manipulator_visualizer_process) != NoneType:
- try:
- self.robot_manager.manipulator_visualizer_process.terminate()
- if self.args.debug_prints:
- print("terminated manipulator_visualizer_process")
- except AttributeError:
- if self.args.debug_prints:
- print("real-time-plotter is dead already")
-
- # let's plot the loop you stopped.
- # --> but then we stop here and can't kill the program with ctrl-c
- # and that is insufferable. if you want the plot open the log
- #for key in self.log_dict:
- # self.log_dict[key] = np.array(self.log_dict[key])
- #plotFromDict(self.log_dict, self.final_iteration, self.args)
+ self.robot_manager.visualizer_manager.terminateProcess()
if not self.args.pinocchio_only:
self.robot_manager.rtde_control.endFreedriveMode()
@@ -468,20 +413,8 @@ class RobotManager:
# start visualize manipulator process if selected.
# has to be started here because it lives throughout the whole run
if args.visualize_manipulator:
- self.manipulator_visualizer_queue = Queue()
- if args.debug_prints:
- print("ROBOT_MANAGER: i created queue for manipulator visualization:", self.manipulator_visualizer_queue)
- print("ROBOT_MANAGER: i am creating and starting the manipulator visualizer process")
- self.manipulator_visualizer_process = Process(target=manipulatorVisualizer,
- args=(self.args, self.model, self.collision_model, self.visual_model, self.manipulator_visualizer_queue, ))
- self.manipulator_visualizer_process.start()
- # give it time to start
- time.sleep(2)
- if args.debug_prints:
- print("ROBOT_MANAGER: manipulator_visualizer_process started")
- self.manipulator_visualizer_queue.put(np.zeros(self.model.nq))
- if args.debug_prints:
- print("ROBOT_MANAGER: i managed to put initializing q to manipulator_visualizer_queue")
+ side_function = partial(manipulatorVisualizer, self.model, self.collision_model, self.visual_model)
+ self.visualizer_manager = ProcessManager(args, side_function, {"q" : np.zeros(self.model.nq)}, 0)
# create log manager if we're saving logs
if args.save_log:
@@ -571,7 +504,7 @@ class RobotManager:
q = np.array(q)
self.q = q
if args.visualize_manipulator:
- self.manipulator_visualizer_queue.put({"q" : q})
+ self.visualizer_manager.sendCommand({"q" : q})
# do it once to get T_w_e
@@ -673,7 +606,6 @@ class RobotManager:
# computeAllTerms is certainly not necessary btw
# but if it runs on time, does it matter? it makes everything available...
# (includes forward kinematics, all jacobians, all dynamics terms, energies)
- # NOTE: it's too slow -> has to have pinocchio with multithreading
#pin.computeAllTerms(self.model, self.data, self.q, self.v_q)
pin.forwardKinematics(self.model, self.data, self.q, self.v_q)
pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
@@ -976,7 +908,7 @@ class RobotManager:
self.Mgoal = Mgoal
if self.args.visualize_manipulator:
# TODO document this somewhere
- self.manipulator_visualizer_queue.put(
+ self.visualizer_manager.sendCommand(
{"Mgoal" : Mgoal})
return Mgoal
@@ -992,14 +924,7 @@ class RobotManager:
RobotManager has to handle the meshcat server.
and in this case the user needs to say when the tasks are done.
"""
- if self.args.debug_prints:
- print("i am putting befree in plotter_queue to stop the manipulator visualizer")
- # putting this command tells our process to kill the meshcat zmq server process
- self.manipulator_visualizer_queue.put_nowait({"befree" : "befree"})
- time.sleep(0.1)
- self.manipulator_visualizer_process.terminate()
- if self.args.debug_prints:
- print("terminated manipulator_visualizer_process")
+ self.visualizer_manager.terminateProcess()
def stopRobot(self):
if not self.args.pinocchio_only:
@@ -1029,7 +954,98 @@ class RobotManager:
because it shouldn't - it should only update the visualizer
"""
if self.args.visualize_manipulator:
- self.manipulator_visualizer_queue.put_nowait(viz_dict)
+ self.visualizer_manager.sendCommand(viz_dict)
else:
if self.args.debug_prints:
print("you didn't select viz")
+
+class ProcessManager:
+ """
+ ProcessManager
+ --------------
+ A way to do processing in a thread (process because GIL) different
+ from the main one which is reserved
+ for ControlLoopManager.
+ The primary goal is to process visual input
+ from the camera without slowing down control.
+ TODO: once finished, map real-time-plotting and
+ visualization with this (it already exists, just not in a manager).
+ What this should do is hide the code for starting a process,
+ and to enforce the communication defined by the user.
+ To make life simpler, all communication is done with Queues.
+ There are two Queues - one for commands,
+ and the other for receiving the process' output.
+ Do note that this is obviously not the silver bullet for every
+ possible inter-process communication scenario,
+ but the aim of this library is to be as simple as possible,
+ not as most performant as possible.
+ NOTE: the maximum number of things in the command queue is arbitrarily
+ set to 5. if you need to get the result immediately,
+ calculate whatever you need in main, not in a side process.
+ this is meant to be used for operations that take a long time
+ and aren't critical, like reading for a camera.
+ """
+ # NOTE: theoretically we could pass existing queues so that we don't
+ # need to create new ones, but i won't optimize in advance
+ def __init__(self, args, side_function, init_command, comm_direction, init_value=None):
+ self.args = args
+
+ if comm_direction == 0:
+ self.command_queue = Queue()
+ self.side_process = Process(target=side_function,
+ args=(args, init_command, self.command_queue,))
+ if comm_direction == 1:
+ self.data_queue = Queue()
+ self.side_process = Process(target=side_function,
+ args=(args, init_command, self.data_queue,))
+ if comm_direction == 2:
+ self.command_queue = Queue()
+ self.data_queue = Queue()
+ self.side_process = Process(target=side_function,
+ args=(args, init_command, self.command_queue, self.data_queue,))
+ if type(side_function) == partial:
+ self.side_process.name = side_function.func.__name__
+ else:
+ self.side_process.name = side_function.__name__ + "_process"
+ self.lastest_data = init_value
+ if self.args.debug_prints:
+ print(f"PROCESS_MANAGER: i created the command queue for {self.side_process.name}", self.command_queue)
+
+ self.side_process.start()
+ if self.args.debug_prints:
+ print("PROCESS_MANAGER: i am starting {self.side_process.name}")
+
+ # TODO: enforce that
+ # the key should be a string containing the command,
+ # and the value should be the data associated with the command,
+ # just to have some consistency
+ def sendCommand(self, command : dict):
+ """
+ sendCommand
+ ------------
+ assumes you're calling from controlLoop and that
+ you want a non-blocking call.
+ the maximum number of things in the command queue is arbitrarily
+ set to 5. if you need to get the result immediately,
+ calculate whatever you need in main, not in a side process.
+ """
+ if self.command_queue.qsize() < 5:
+ self.command_queue.put_nowait(command)
+
+ def getData(self):
+ if not self.data_queue.empty():
+ self.lastest_data = self.data_queue.get_nowait()
+ return self.lastest_data.copy()
+
+ def terminateProcess(self):
+ if self.args.debug_prints:
+ print(f"i am putting befree in {self.side_process.name}'s command queue to stop it")
+ self.command_queue.put_nowait("befree")
+ try:
+ self.side_process.terminate()
+ if self.args.debug_prints:
+ print(f"terminated {self.side_process.name}")
+ except AttributeError:
+ if self.args.debug_prints:
+ print(f"{self.side_process.name} is dead already")
+
diff --git a/python/ur_simple_control/optimal_control/create_pinocchio_casadi_ocp.py b/python/ur_simple_control/optimal_control/create_pinocchio_casadi_ocp.py
index d0e27178b6e2b8120247fa7ec1d8b3011bc72b5b..a11cf65d5106aecdea4b8e4244e724196a4aa265 100644
--- a/python/ur_simple_control/optimal_control/create_pinocchio_casadi_ocp.py
+++ b/python/ur_simple_control/optimal_control/create_pinocchio_casadi_ocp.py
@@ -3,8 +3,11 @@ import pinocchio as pin
from pinocchio import casadi as cpin
import numpy as np
from ur_simple_control.managers import RobotManager, ControlLoopManager
+from ur_simple_control.util.encapsulating_ellipses import computeEncapsulatingEllipses, visualizeEllipses
import pickle
from importlib.resources import files
+from types import SimpleNamespace
+from os import path
"""
CREDIT: jrnh2023 pinocchio.casadi tutorial
@@ -29,104 +32,208 @@ The following tools are used:
It assumes that the ellipses parameters are already computed, see ellipses.py for that.
"""
-def createCasadiIKObstacleAvoidanceOCP(args, robot : RobotManager, x0, goal: pin.SE3):
- # TODO have ellipses in robot.ellipses. this is loaded if you load whatever argument is responsible
- # or you load them here from a file in robot_descriptions
- # which you need to generate from generate_ellipses.py in utils
- # there should be one for each link more or less
- # TODO: make this for every robot.
- # TODO; calculate ellipses if they don't exist already
+# TODO: finish and verify
+# make a separate function for a single ellipse based on the namespace
+# if necessary save more info from solver
+# --> memory is cheaper than computation,
+# don't save something only if it makes the struct confusing
+def getSelfCollisionObstacles(args, robot : RobotManager):
ellipses_path = files('ur_simple_control.robot_descriptions.ellipses').joinpath("ur5e_robotiq_ellipses.pickle")
- file = open(ellipses_path, 'rb')
- ellipses = pickle.load(file)
- file.close()
+ if path.exists(ellipses_path):
+ file = open(ellipses_path, 'rb')
+ ellipses = pickle.load(file)
+ file.close()
+ else:
+ ellipses = computeEncapsulatingEllipses(args, robot)
for ellipse in ellipses:
- ellipse.id = model.getJointId(ellipse.name)
+ ellipse.id = robot.model.getJointId(ellipse.name)
l, P = np.linalg.eig(ellipse.A)
ellipse.radius = 1 / l**0.5
ellipse.rotation = P
- # TODO: use wrapped meshcat visualizer to have access to more nifty plotting
- #ellipse.placement = pin.SE3(P, ellipse.center)
- #viz.addEllipsoid(f"el_{e.name}", e.radius, [0.3, 0.9, 0.3, 0.3])
+
+ # TODO: move from here, you want this updated.
+ # so it should be handled by some
+ #if args.visualize_ellipses:
+ # visualizeEllipses(args, robot, ellipses)
+
+ return ellipses
+
+def createCasadiIKObstacleAvoidanceOCP(args, robot : RobotManager, T_goal: pin.SE3):
+ # casadi equivalents of core pinocchio classes (used for everything)
+ cmodel = cpin.Model(robot.model)
+ cdata = cmodel.createData()
+ nq = robot.model.nq
+ nv = robot.model.nv
+ q0 = robot.getQ()
+ v0 = robot.getQd()
+ # casadi symbolic variables for joint angles q
+ cq = casadi.SX.sym("q", robot.model.nq, 1)
+ cv = casadi.SX.sym("v", robot.model.nv, 1)
+ # generate forward kinematics in casadi
+ cpin.framesForwardKinematics(cmodel, cdata, cq)
+
+ # kinematics constraint
+ cnext = casadi.Function(
+ "next",
+ [cq, cv],
+ [cpin.integrate(cmodel, cq, cv * args.ocp_dt)]
+ )
+
+ # error to goal function
+ error6_tool = casadi.Function(
+ "etool",
+ [cq],
+ [cpin.log6(cdata.oMf[robot.ee_frame_id].inverse() * cpin.SE3(T_goal)).vector],
+ )
+
# TODO: obstacles
# here you need to define other obstacles, namely the table (floor)
# it's obviously going to be a plane
# alternatively just forbid z-axis of end-effector to be negative
obstacles = []
+# obstacles = [
+# SimpleNamespace(radius=0.01, pos=np.array([-0.4, 0.2 + s, 0.5]), name=f"obs_{i_s}")
+# for i_s, s in enumerate(np.arange(-0.5, 0.5, 0.25))
+# ]
+ # define the optimizer/solver
+ opti = casadi.Opti()
- cmodel = cpin.Model(robot.model)
- cdata = cmodel.createData()
+ # one set of decision variables per knot (both states and control input)
+ var_qs = [opti.variable(nq) for t in range(args.n_knots + 1)]
+ var_vs = [opti.variable(nv) for t in range(args.n_knots)]
+
+ # running costs - x**2 and u**2 - used more for regularization than anything else
+ totalcost = 0
+ for t in range(args.n_knots):
+ # running
+ totalcost += 1e-3 * args.ocp_dt * casadi.sumsqr(var_qs[t])
+ totalcost += 1e-4 * args.ocp_dt * casadi.sumsqr(var_vs[t])
+ # terminal cost
+ totalcost += 1e4 * casadi.sumsqr(error6_tool(var_qs[args.n_knots]))
- #######################################################################
- # kinematics level #
- #######################################################################
-
-# cq = casadi.SX.sym("q", robot.model.nq, 1)
-# cpin.framesForwardKinematics(cmodel, cdata, cq)
-# error6_tool = casadi.Function(
-# "etool",
-# [cq],
-# [cpin.log6(cdata.oMf[robot.ee_frame_id].inverse() * cpin.SE3(Mtarget)).vector],
-# )
+
+ # initial state constraints
+ # TODO: idk if you need to input x0 in this way, maybe robot.getQ(d) is better?
+ opti.subject_to(var_qs[0] == q0)
+ opti.subject_to(var_vs[0] == v0)
+ # kinematics constraints between every knot pair
+ for t in range(args.n_knots):
+ opti.subject_to(cnext(var_qs[t], var_vs[t]) == var_qs[t + 1])
+ opti.subject_to(var_vs[t] <= np.ones(robot.model.nv) * robot.max_qd)
+ opti.subject_to(var_vs[t] >= -1 * np.ones(robot.model.nv) * robot.max_qd)
+
+ # obstacle avoidance (hard) constraints: no ellipse should intersect any of the obstacles
+ ellipses = getSelfCollisionObstacles(args, robot)
+ cpos = casadi.SX.sym("p", 3) # obstacle position in ellipse frame
for ellipse in ellipses:
- # Position of the obstacle cpos in the ellipse frame.
ellipse.e_pos = casadi.Function(
- f"e{e.id}", [cq, cpos], [cdata.oMi[ellipse.id].inverse().act(casadi.SX(cpos))]
+ f"e{ellipse.name}", [cq, cpos], [cdata.oMi[ellipse.id].inverse().act(casadi.SX(cpos))]
)
- #######################################################################
- # dynamics level #
- #######################################################################
+ for obstacle in obstacles:
+ for q in var_qs:
+ # obstacle position in ellipsoid (joint) frame
+ #e_pos = e.e_pos(var_q, o.pos)
+ e_pos = ellipse.e_pos(q, obstacle.pos)
+ opti.subject_to((e_pos - ellipse.center).T @ ellipse.A @ (e_pos - ellipse.center) >= 1)
- # variables on a single time-step
+ # now that the ocp has been transcribed as nlp,
+ # solve it
+ opti.minimize(totalcost)
+ opti.minimize(totalcost)
+ p_opts = dict(print_time=False, verbose=False)
+ s_opts = dict(print_level=0)
+ opti.solver("ipopt") # set numerical backend
+ opti.set_initial(var_qs[0], q0)
+ try:
+ sol = opti.solve_limited()
+ #sol_q = opti.value(var_q)
+ sol_qs = [opti.value(var_q) for var_q in var_qs]
+ sol_vs = [opti.value(var_v) for var_v in var_vs]
+ except:
+ print("ERROR in convergence, plotting debug info.")
+ #sol_q = opti.debug.value(var_q)
+ sol_qs = [opti.debug.value(var_q) for var_q in var_qs]
+ sol_vs = [opti.debug.value(var_v) for var_v in var_vs]
+
+ reference = {'qs' : sol_qs, 'vs' : sol_vs, 'dt' : args.ocp_dt}
+ return reference, opti
+
+def createCasadiReachingObstacleAvoidanceDynamicsOCP(args, robot : RobotManager, x0, goal : pin.SE3):
+ # state = [joint_positions, joint_velocities], i.e. x = [q, v].
+ # the velocity dimension does not need to be the same as the positions,
+ # ex. consider a differential drive robot: it covers [x,y,theta], but has [vx, vtheta] as velocity
nx = robot.model.nq + robot.model.nv
ndx = 2 * robot.model.nv
+ x0 = np.concat((robot.getQ(), robot.getQd()))
cx = casadi.SX.sym("x", nx, 1)
+ # dx = [velocity, acceleration] = [v,a]
+ # acceleration is the same dimension as velocity
cdx = casadi.SX.sym("dx", robot.model.nv * 2, 1)
+ # separate state for less cumbersome expressions (they're still the same casadi variables)
cq = cx[:robot.model.nq]
cv = cx[robot.model.nq:]
+ # acceleration is the same dimension as velocity
caq = casadi.SX.sym("a", robot.model.nv, 1)
cpin.forwardKinematics(cmodel, cdata, cq, cv, caq)
cpin.updateFramePlacements(cmodel, cdata)
- # cost function - reaching goal
- error_tool = casadi.Function(
- "etool6", [cx], [cpin.log6(cdata.oMf[endEffector_ID].inverse() * cpin.SE3(Mtarget)).vector],)
-
- # nonlinear dynamics constraint
+ # dynamics constraint
cnext = casadi.Function(
"next",
[cx, caq],
[
casadi.vertcat(
- cpin.integrate(cmodel, cx[:robot.model.nq], cx[robot.model.nq:] * args.ocp_dt + caq * args.ocp_dt**2),
- cx[nq:] + caq * args.ocp_dt,
+ # TODO: i'm not sure i need to have the acceleration update for the position
+ # because i am integrating the velocity as well.
+ # i guess this is the way to go because we can't first integrate
+ # the velocity and then the position, but have to do both simultaneously
+ cpin.integrate(cmodel, cq, cv * args.ocp_dt + caq * args.ocp_dt**2),
+ cv + caq * args.ocp_dt
)
],
)
- opti = casadi.Opti()
+
+ # cost function - reaching goal
+ error6_tool = casadi.Function(
+ "etool6",
+ [cx],
+ [cpin.log6(cdata.oMf[endEffector_ID].inverse() * cpin.SE3(Mtarget)).vector],)
+
+ # TODO: obstacles
+ # here you need to define other obstacles, namely the table (floor)
+ # it's obviously going to be a plane
+ # alternatively just forbid z-axis of end-effector to be negative
+ obstacles = []
# one set of decision variables per knot (both states and control input)
var_xs = [opti.variable(nx) for t in range(args.n_knots + 1)]
var_as = [opti.variable(nv) for t in range(args.n_knots)]
- # running costs - x**2 and u**@ - used more for regularization than anything else
+ # running costs - x**2 and u**2 - used more for regularization than anything else
totalcost = 0
- for t in range(T):
+ for t in range(args.n_knots):
# running
- totalcost += 1e-3 * DT * casadi.sumsqr(var_xs[t][nq:])
- totalcost += 1e-4 * DT * casadi.sumsqr(var_as[t])
+ totalcost += 1e-3 * args.ocp_dt * casadi.sumsqr(var_xs[t][nq:])
+ totalcost += 1e-4 * args.ocp_dt * casadi.sumsqr(var_as[t])
# terminal cost
- totalcost += 1e4 * casadi.sumsqr(error_tool(var_xs[T]))
-
+ totalcost += 1e4 * casadi.sumsqr(error6_tool(var_xs[T]))
+
+ # now we combine the components into the OCP
+ opti = casadi.Opti()
- # collision avoidance (hard) constraints
+ # initial state constraints
# TODO: idk if you need to input x0 in this way, maybe robot.getQ(d) is better?
opti.subject_to(var_xs[0][:nq] == x0[:robot.model.nq])
opti.subject_to(var_xs[0][nq:] == x0[robot.model.nq:])
- for t in range(T):
+ # dynamics constraint between every knot pair
+ for t in range(args.n_knots):
opti.subject_to(cnext(var_xs[t], var_as[t]) == var_xs[t + 1])
+
+ # obstacle avoidance (hard) constraints: no ellipse should intersect any of the obstacles
+ ellipses = getSelfCollisionObstacles(args, robot)
+ cpos = casadi.SX.sym("p", 3)
for ellipse in ellipses:
for obstacle in obstacles:
for x in var_xs:
@@ -137,6 +244,12 @@ def createCasadiIKObstacleAvoidanceOCP(args, robot : RobotManager, x0, goal: pin
# now that the ocp has been transcribed as nlp,
# solve it
+ opti.minimize(totalcost)
+ opti.minimize(totalcost)
+ p_opts = dict(print_time=False, verbose=False)
+ s_opts = dict(print_level=0)
+ opti.solver("ipopt") # set numerical backend
+ opti.set_initial(var_qs[0], q0)
try:
sol = opti.solve_limited()
#sol_q = opti.value(var_q)
@@ -147,3 +260,8 @@ def createCasadiIKObstacleAvoidanceOCP(args, robot : RobotManager, x0, goal: pin
#sol_q = opti.debug.value(var_q)
sol_xs = [opti.debug.value(var_x) for var_x in var_xs]
sol_as = [opti.debug.value(var_a) for var_a in var_as]
+ # TODO: extract qs and vs from xs, put in as
+ # TODO: write dynamic trajectory following
+ reference = {'qs' : sol_qs, 'vs' : sol_vs, 'dt' : args.ocp_dt}
+ return reference, opti
+ return sol_xs, sol_as
diff --git a/python/ur_simple_control/util/encapsulating_ellipses.py b/python/ur_simple_control/util/encapsulating_ellipses.py
index 3e20424801bdd1e460435405d9684c4adaf380ab..7515bfc96dad5974a9c345ea103954089b0171b3 100644
--- a/python/ur_simple_control/util/encapsulating_ellipses.py
+++ b/python/ur_simple_control/util/encapsulating_ellipses.py
@@ -6,36 +6,46 @@ Used to construct ellipses around manipulator links to get convex self-collision
NOTE: probably cylinders are better but let's run with this.
decide:
- w in so3: ellipse orientation
- - r in r3: ellipse main dimensions
+ - r in R^3: ellipse main dimensions
minimizing:
- r1*r2*r3 the volum of the ellipse
+ r1*r2*r3 the volum of the ellipse -> possibly not really (can calculate but eh) but it leads to the same result
so that:
r>=0
- for all points pk in a list, pk in ellipse: (pk-c)@A@pk-c)<=1
+ for all points x in a list, x in ellipse: (x - c) @ A @ (x - c) <= 1
with A,c the matrix representation of the ellipsoid A=exp(w)@diag(1/r**2)@exp(w).T
-Once solved, this can be stored instead of recomputed all the time.
+Once solved, this is stored so it doesn't need to be recomputed every the time.
"""
+# BIG TODO: make gripper one big ellipse
+# going through every link is both wrong and pointlessly complex
+# also TODO: try fitting cyllinders instead
import casadi
import numpy as np
import pinocchio as pin
from pinocchio import casadi as cpin
from ur_simple_control.visualize.meshcat_viewer_wrapper.visualizer import MeshcatVisualizer
+from ur_simple_control.managers import RobotManager
from ur_simple_control.util.get_model import get_model, get_heron_model
from types import SimpleNamespace
import time
import pickle
from importlib.resources import files
-# plotting the ellipse in matplotlib for verification
-def plotEllipse(ax, opti_A, opti_c):
+
+import pinocchio as pin
+import numpy as np
+import time
+import argparse
+from functools import partial
+from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
+
+def plotEllipseMatplotlib(ax, opti_A, opti_c):
# find the rotation matrix and radii of the axes
U, s, rotation = np.linalg.svd(opti_A)
radii = 1.0 / np.sqrt(s)
- # now carry on with EOL's answer
u = np.linspace(0.0, 2.0 * np.pi, 100)
v = np.linspace(0.0, np.pi, 100)
x = radii[0] * np.outer(np.cos(u), np.sin(v))
@@ -47,9 +57,19 @@ def plotEllipse(ax, opti_A, opti_c):
np.dot([x[i, j], y[i, j], z[i, j]], rotation) + opti_c
)
- # plot
ax.plot_wireframe(x, y, z, rstride=4, cstride=4, color="b", alpha=0.2)
+# TODO: finish and verify
+# make a separate function for a single ellipse based on the namespace
+def visualizeEllipses(args, robot : RobotManager, ellipses):
+ e, P = np.linalg.eig()
+ ellipse_placement = pin.SE3(P, ellipse.center)
+ #ellipse_placement = data.oMf[geom.parentFrame].act(geom.placement.act(ellipse_placement))
+ ellipse_placement = data.oMf[geom.parentFrame].act(ellipse_placement)
+ # TODO: add to MeshcatVisualizerWrapper,
+ # add queue handling in visualize function
+ viz.addEllipsoid(f"el_{ellipse.name}", sol_r, [0.3, 0.9, 0.3, 0.3])
+ viz.applyConfiguration(f"el_{ellipse.name}", ellipse_placement)
# plotting the vertices in the ellipse in matplotlib for verification
def plotVertices(ax, vertices, nsubsample):
@@ -78,108 +98,173 @@ def plotVertices(ax, vertices, nsubsample):
bottom=plot_center[2] - plot_length, top=plot_center[2] + plot_length
)
-model, collision_model, visual_model, data = get_model()
-#viz = MeshcatVisualizer(model=model, collision_model=collision_model, visual_model=visual_model)
-#viz.display(np.zeros(model.nq))
+def visualizeVertices(args, robot : RobotManager):
-ellipses = []
-for geom in collision_model.geometryObjects:
- vertices = geom.geometry.vertices()
+ for i, geom in enumerate(robot.collision_model.geometryObjects):
+ vertices = geom.geometry.vertices()
+
+ for i in np.arange(0, vertices.shape[0]):
+ viz.addSphere(f"world/point_{i}", 5e-3, [1, 0, 0, 0.8])
+ vertix_pose = pin.SE3.Identity()
+ vertix_pose.translation = vertices[i]
+ #vertix_pose = data.oMi[geom.parentJoint].act(vertix_pose)
+ vertix_pose = data.oMf[geom.parentFrame].act(geom.placement.act(vertix_pose))
+ #viz.applyConfiguration(f"world/point_{i}", np.array(vertices[i].tolist() + [1, 0, 0, 0]))
+ viz.applyConfiguration(f"world/point_{i}", vertix_pose)
-# for i in np.arange(0, vertices.shape[0]):
-# viz.addSphere(f"world/point_{i}", 5e-3, [1, 0, 0, 0.8])
-# viz.applyConfiguration(f"world/point_{i}", vertices[i].tolist() + [1, 0, 0, 0])
- cw = casadi.SX.sym("w", 3)
- exp = casadi.Function("exp3", [cw], [cpin.exp3(cw)])
+# TODO: make this for every robot.
+def computeEncapsulatingEllipses(args, robot : RobotManager):
+ model, collision_model, visual_model, data = (robot.model, robot.collision_model, robot.visual_model, robot.data)
+ viz = MeshcatVisualizer(model=model, collision_model=collision_model, visual_model=visual_model)
+ #q = np.zeros(model.nq)
+ q = pin.randomConfiguration(model)
+ print(q)
+ viz.display(q)
+ pin.forwardKinematics(model, data, q)
+ pin.updateFramePlacements(model, data)
+ pin.updateGlobalPlacements(model,data)
+ pin.computeAllTerms(model,data,q,np.zeros(model.nv))
+ time.sleep(3)
+
+ ellipses = []
+ for i, geom in enumerate(collision_model.geometryObjects):
+ vertices = geom.geometry.vertices()
+
+ for i in np.arange(0, vertices.shape[0]):
+ viz.addSphere(f"world/point_{i}", 5e-3, [1, 0, 0, 0.8])
+ vertix_pose = pin.SE3.Identity()
+ vertix_pose.translation = vertices[i]
+ #vertix_pose = data.oMi[geom.parentJoint].act(vertix_pose)
+ vertix_pose = data.oMf[geom.parentFrame].act(geom.placement.act(vertix_pose))
+ #viz.applyConfiguration(f"world/point_{i}", np.array(vertices[i].tolist() + [1, 0, 0, 0]))
+ viz.applyConfiguration(f"world/point_{i}", vertix_pose)
+
+ cw = casadi.SX.sym("w", 3)
+ exp = casadi.Function("exp3", [cw], [cpin.exp3(cw)])
+
+ """
+ decide
+ - w in so3: ellipse orientation
+ - r in r3: ellipse main dimensions
+ minimizing:
+ r1*r2*r3 the volum of the ellipse
+ so that:
+ r>=0
+ for all points pk in a list, pk in ellipse
+
+ """
+ opti = casadi.Opti()
+ var_w = opti.variable(3)
+ var_r = opti.variable(3)
+ var_c = opti.variable(3)
+
+ # The ellipsoid matrix is represented by w=log3(R),diag(P) with R,P=eig(A)
+ R = exp(var_w)
+ A = R @ casadi.diag(1 / var_r**2) @ R.T
+
+ totalcost = var_r[0] * var_r[1] * var_r[2]
+
+ opti.subject_to(var_r >= 0)
+
+ for g_v in vertices:
+ # g_v is the vertex v expressed in the geometry frame.
+ # Convert point from geometry frame to joint frame
+ j_v = geom.placement.act(g_v)
+ # Constraint the ellipsoid to be including the point
+ opti.subject_to((j_v - var_c).T @ A @ (j_v - var_c) <= 1)
+
+ ### SOLVE
+ opti.minimize(totalcost)
+ opti.solver("ipopt") # set numerical backend
+ opti.set_initial(var_r, 10)
+
+ sol = opti.solve_limited()
+
+ sol_r = opti.value(var_r)
+ sol_A = opti.value(A)
+ sol_c = opti.value(var_c)
+ sol_R = opti.value(exp(var_w))
+
+ # TODO: add placement=pin.SE3(P, ellipse.center) and id=robot.model.getJointId(e.name)
+ ellipse = SimpleNamespace(
+ name=model.names[geom.parentJoint],
+ A=sol_A,
+ center=sol_c)
+ ellipses.append(ellipse)
+ e, P = np.linalg.eig(sol_A)
+ ellipse_placement = pin.SE3(P, ellipse.center)
+ #ellipse_placement = data.oMf[geom.parentFrame].act(geom.placement.act(ellipse_placement))
+ ellipse_placement = data.oMf[geom.parentFrame].act(ellipse_placement)
+ viz.addEllipsoid(f"el_{ellipse.name}", sol_r, [0.3, 0.9, 0.3, 0.3])
+ viz.applyConfiguration(f"el_{ellipse.name}", ellipse_placement)
+ print(ellipse)
+
+ ellipses_path = files('ur_simple_control.robot_descriptions.ellipses').joinpath("ur5e_robotiq_ellipses.pickle")
+ file = open(ellipses_path, 'wb')
+ pickle.dump(ellipses, file)
+ file.close()
+
+ # Recover r,R from A (for fun)
+ # e, P = np.linalg.eig(sol_A)
+ # recons_r = 1 / e**0.5
+ # recons_R = P
+ #
+ # # Build the ellipsoid 3d shape
+ # # Ellipsoid in meshcat
+ # viz.addEllipsoid("el", sol_r, [0.3, 0.9, 0.3, 0.3])
+ # # jMel is the placement of the ellipsoid in the joint frame
+ # jMel = pin.SE3(sol_R, sol_c)
+ #
+ # # Place the body, the vertices and the ellispod at a random configuration oMj_rand
+ # oMj_rand = pin.SE3.Random()
+ # viz.applyConfiguration(viz.getViewerNodeName(geom, pin.VISUAL), oMj_rand)
+ # for i in np.arange(0, vertices.shape[0]):
+ # viz.applyConfiguration(
+ # f"world/point_{i}", oMj_rand.act(vertices[i]).tolist() + [1, 0, 0, 0]
+ # )
+ # viz.applyConfiguration("el", oMj_rand * jMel)
+ #
+ # print(
+ # f'SimpleNamespace(name="{model.names[geom.parentJoint]}",\n'
+ # + f" A=np.{repr(sol_A)},\n"
+ # + f" center=np.{repr(sol_c)})"
+ # )
+ # time.sleep(5)
+ # Matplotlib (for fun)
+ #import matplotlib.pyplot as plt
+ #
+ #plt.ion()
+ #from utils.plot_ellipse import plotEllipse, plotVertices
+
+ #fig, ax = plt.subplots(1, subplot_kw={"projection": "3d"})
+ #plotEllipse(ax, sol_A, sol_c)
+ #plotVertices(ax, np.vstack([geom.placement.act(p) for p in vertices]), 1)
- """
- decide
- - w in so3: ellipse orientation
- - r in r3: ellipse main dimensions
- minimizing:
- r1*r2*r3 the volum of the ellipse
- so that:
- r>=0
- for all points pk in a list, pk in ellipse
- """
- opti = casadi.Opti()
- var_w = opti.variable(3)
- var_r = opti.variable(3)
- var_c = opti.variable(3)
-
- # The ellipsoid matrix is represented by w=log3(R),diag(P) with R,P=eig(A)
- R = exp(var_w)
- A = R @ casadi.diag(1 / var_r**2) @ R.T
-
- totalcost = var_r[0] * var_r[1] * var_r[2]
-
- opti.subject_to(var_r >= 0)
-
- for g_v in vertices:
- # g_v is the vertex v expressed in the geometry frame.
- # Convert point from geometry frame to joint frame
- j_v = geom.placement.act(g_v)
- # Constraint the ellipsoid to be including the point
- opti.subject_to((j_v - var_c).T @ A @ (j_v - var_c) <= 1)
-
- ### SOLVE
- opti.minimize(totalcost)
- opti.solver("ipopt") # set numerical backend
- opti.set_initial(var_r, 10)
-
- sol = opti.solve_limited()
-
- sol_r = opti.value(var_r)
- sol_A = opti.value(A)
- sol_c = opti.value(var_c)
- sol_R = opti.value(exp(var_w))
-
- ellipse = SimpleNamespace(
- name=model.names[geom.parentJoint],
- A=sol_A,
- center=sol_c)
- ellipses.append(ellipse)
- print(ellipse)
-
-ellipses_path = files('ur_simple_control.robot_descriptions.ellipses').joinpath("ur5e_robotiq_ellipses.pickle")
-file = open(ellipses_path, 'wb')
-pickle.dump(ellipses, file)
-file.close()
-
- # Recover r,R from A (for fun)
-# e, P = np.linalg.eig(sol_A)
-# recons_r = 1 / e**0.5
-# recons_R = P
-#
-# # Build the ellipsoid 3d shape
-# # Ellipsoid in meshcat
-# viz.addEllipsoid("el", sol_r, [0.3, 0.9, 0.3, 0.3])
-# # jMel is the placement of the ellipsoid in the joint frame
-# jMel = pin.SE3(sol_R, sol_c)
-#
-# # Place the body, the vertices and the ellispod at a random configuration oMj_rand
-# oMj_rand = pin.SE3.Random()
-# viz.applyConfiguration(viz.getViewerNodeName(geom, pin.VISUAL), oMj_rand)
-# for i in np.arange(0, vertices.shape[0]):
-# viz.applyConfiguration(
-# f"world/point_{i}", oMj_rand.act(vertices[i]).tolist() + [1, 0, 0, 0]
-# )
-# viz.applyConfiguration("el", oMj_rand * jMel)
-#
-# print(
-# f'SimpleNamespace(name="{model.names[geom.parentJoint]}",\n'
-# + f" A=np.{repr(sol_A)},\n"
-# + f" center=np.{repr(sol_c)})"
-# )
-# time.sleep(5)
- # Matplotlib (for fun)
- #import matplotlib.pyplot as plt
- #
- #plt.ion()
- #from utils.plot_ellipse import plotEllipse, plotVertices
+def get_args():
+ parser = getMinimalArgParser()
+ parser.description = 'describe this example'
+ # add more arguments here from different Simple Manipulator Control modules
+ args = parser.parse_args()
+ return args
+
+if __name__ == "__main__":
+ args = get_args()
+ robot = RobotManager(args)
+
+ computeEncapsulatingEllipses(args,robot)
+
+ # get expected behaviour here (library can't know what the end is - you have to do this here)
+ if not args.pinocchio_only:
+ robot.stopRobot()
+
+ if args.save_log:
+ robot.log_manager.plotAllControlLoops()
- #fig, ax = plt.subplots(1, subplot_kw={"projection": "3d"})
- #plotEllipse(ax, sol_A, sol_c)
- #plotVertices(ax, np.vstack([geom.placement.act(p) for p in vertices]), 1)
+ if args.visualize_manipulator:
+ robot.killManipulatorVisualizer()
+
+ if args.save_log:
+ robot.log_manager.saveLog()
+ #loop_manager.stopHandler(None, None)
diff --git a/python/ur_simple_control/vision/vision.py b/python/ur_simple_control/vision/vision.py
new file mode 100644
index 0000000000000000000000000000000000000000..d2a0c2085faa36712930852b0265d7d43c906479
--- /dev/null
+++ b/python/ur_simple_control/vision/vision.py
@@ -0,0 +1,32 @@
+import cv2
+from multiprocessing import Queue
+import numpy as np
+
+def processCamera(device, args, cmd, queue : Queue):
+ camera = cv2.VideoCapture(0)
+ # if you have ip_webcam set-up you can do this (but you have put in the correct ip)
+ #camera = cv2.VideoCapture("http://192.168.219.153:8080/video")
+
+ try:
+ while True:
+ (grabbed, frame) = camera.read()
+ # frame = cv2.flip(frame, 1)
+ scale_percent = 100 # percent of original size
+ width = int(frame.shape[1] * scale_percent / 100)
+ height = int(frame.shape[0] * scale_percent / 100)
+ dim = (width, height)
+
+ # resize image
+ frame = cv2.resize(frame, dim, interpolation = cv2.INTER_AREA)
+ # frame = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+ cv2.imshow("camera", frame)
+ keypress = cv2.waitKey(1) & 0xFF
+
+ if keypress == ord("q"):
+ break
+
+ queue.put_nowait({"x" : np.random.randint(0, 10),
+ "y" : np.random.randint(0, 10)})
+ except KeyboardInterrupt:
+ if args.debug_prints:
+ print("PROCESS_CAMERA: caught KeyboardInterrupt, i'm out")
diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc
index a5a1f53946b06f0ead307dfed8f5505c1e54eb9a..da6b4e16226e0ccf7592148fce5af4781ca85e75 100644
Binary files a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc and b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc differ
diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py
index 4cf8d138f7b5685fddd9b9059c36c148eb6e6756..f39cf81bca10106a1b469068e91dbf4aaef24b42 100644
--- a/python/ur_simple_control/visualize/visualize.py
+++ b/python/ur_simple_control/visualize/visualize.py
@@ -64,7 +64,7 @@ def plotFromDict(plot_data, final_iteration, args, title="title"):
# STUPID MATPLOTLIB CAN'T HANDLE MULTIPLE FIGURES FROM DIFFERENT PROCESS
# LITERALLY REFUSES TO GET ME A FIGURE
-def realTimePlotter(args, queue):
+def realTimePlotter(args, log_item, queue):
"""
realTimePlotter
---------------
@@ -73,22 +73,25 @@ def realTimePlotter(args, queue):
"""
if args.debug_prints:
print("REAL_TIME_PLOTTER: i got this queue:", queue)
+ # NOTE: CAN'T PLOT ANYTHING BEFORE, THIS HANGS IF YOU HAD A PLOT BEFORE
plt.ion()
fig = plt.figure()
canvas = fig.canvas
- queue.put("success")
+# if args.debug_prints:
+# print("REAL_TIME_PLOTTER: putting success into queue")
+ #queue.put("success")
logs_deque = {}
logs_ndarrays = {}
AxisAndArtists = namedtuple("AxAndArtists", "ax artists")
axes_and_updating_artists = {}
- if args.debug_prints:
- print("REAL_TIME_PLOTTER: i am waiting for the first log_item to initialize myself")
- log_item = queue.get()
+ #if args.debug_prints:
+ # print("REAL_TIME_PLOTTER: i am waiting for the first log_item to initialize myself")
+ #log_item = queue.get()
if len(log_item) == 0:
print("you've send me nothing, so no real-time plotting for you")
return
- if args.debug_prints:
- print("REAL_TIME_PLOTTER: got log_item, i am initializing the desired plots")
+ #if args.debug_prints:
+ # print("REAL_TIME_PLOTTER: got log_item, i am initializing the desired plots")
ROLLING_BUFFER_SIZE = 100
t = np.arange(ROLLING_BUFFER_SIZE)
@@ -162,7 +165,7 @@ def realTimePlotter(args, queue):
plt.close(fig)
-def manipulatorVisualizer(args, model, collision_model, visual_model, queue):
+def manipulatorVisualizer(model, collision_model, visual_model, args, cmd, queue):
viz = MeshcatVisualizer(model=model, collision_model=collision_model, visual_model=visual_model)
viz.loadViewerModel()
# set shapes we know we'll use