From 37f31a977fa6f23c821d2481ca8e37e46b0b8826 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Mon, 18 Nov 2024 19:09:19 +0100
Subject: [PATCH] getting closer to using casadi withing SMC. fixed opencv with
conda. ellipses put in but untested. added something to __init__.py's,
hopefully more usable now but definitely not done. whatever was last changed
in drawing example. freedrive_v2 is here - it's freedrive but in controlLoop
to get all of those benefits.
---
Dockerfile | 3 +-
docs/installation.md | 4 +-
python/convenience_tool_box/freedrive_v2.0.py | 4 +
.../open_close_gripper.py | 52 +-
.../casadi_ocp_collision_avoidance.py | 34 +
python/examples/crocoddyl_mpc.py | 3 +-
python/examples/crocoddyl_ocp_clik.py | 3 +-
python/examples/drawing_from_input_drawing.py | 80 +--
python/examples/joint_trajectory.csv | 667 ++++++++++--------
python/examples/path_in_pixels.csv | 667 ++++++++++--------
python/ur_simple_control/__init__.py | 2 +-
.../__pycache__/__init__.cpython-312.pyc | Bin 391 -> 391 bytes
.../__pycache__/managers.cpython-312.pyc | Bin 47221 -> 47217 bytes
python/ur_simple_control/basics/__init__.py | 1 +
.../__pycache__/__init__.cpython-312.pyc | Bin 209 -> 237 bytes
python/ur_simple_control/clik/__init__.py | 1 +
.../clik/__pycache__/__init__.cpython-312.pyc | Bin 207 -> 233 bytes
python/ur_simple_control/clik/clik.py | 30 +-
python/ur_simple_control/dmp/__init__.py | 1 +
.../dmp/__pycache__/__init__.cpython-312.pyc | Bin 206 -> 231 bytes
python/ur_simple_control/managers.py | 8 +-
.../optimal_control/__init__.py | 4 +
.../create_pinocchio_casadi_ocp.py | 149 ++++
.../crocoddyl_optimal_control.py | 15 -
.../optimal_control/get_ocp_args.py | 15 +
python/ur_simple_control/util/__init__.py | 8 +
.../util/__pycache__/__init__.cpython-312.pyc | Bin 207 -> 207 bytes
.../calib_board_hacks.cpython-312.pyc | Bin 13445 -> 13443 bytes
.../__pycache__/get_model.cpython-312.pyc | Bin 8245 -> 8617 bytes
.../util/calib_board_hacks.py | 3 -
.../util/encapsulating_ellipses.py | 185 +++++
python/ur_simple_control/util/get_model.py | 17 +
.../util/grippers/__init__.py | 0
.../__pycache__/visualize.cpython-312.pyc | Bin 17531 -> 16894 bytes
.../meshcat_viewer_wrapper/__init__.py | 0
.../meshcat_viewer_wrapper/colors.py | 49 ++
.../meshcat_viewer_wrapper/visualizer.py | 103 +++
.../ur_simple_control/visualize/visualize.py | 32 +-
38 files changed, 1381 insertions(+), 759 deletions(-)
create mode 100644 python/examples/casadi_ocp_collision_avoidance.py
create mode 100644 python/ur_simple_control/optimal_control/__init__.py
create mode 100644 python/ur_simple_control/optimal_control/create_pinocchio_casadi_ocp.py
create mode 100644 python/ur_simple_control/optimal_control/get_ocp_args.py
create mode 100644 python/ur_simple_control/util/encapsulating_ellipses.py
create mode 100644 python/ur_simple_control/util/grippers/__init__.py
create mode 100644 python/ur_simple_control/visualize/meshcat_viewer_wrapper/__init__.py
create mode 100644 python/ur_simple_control/visualize/meshcat_viewer_wrapper/colors.py
create mode 100644 python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py
diff --git a/Dockerfile b/Dockerfile
index 79df137..ac05d11 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -25,6 +25,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \
vim-python-jedi \
zsh \
zsh-syntax-highlighting \
+ libarchive-tools \
python3-python-qt-binding
# qt-binding is a really unnecessary 300MB, but i don't want
@@ -74,6 +75,6 @@ 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 -y pinocchio -c conda-forge
+RUN conda install --solver=classic -y pinocchio -c conda-forge
RUN pip install matplotlib meshcat ur_rtde \
qpsolvers ecos example_robot_data meshcat_shapes
diff --git a/docs/installation.md b/docs/installation.md
index ff8d3d2..3d92bd0 100644
--- a/docs/installation.md
+++ b/docs/installation.md
@@ -47,7 +47,9 @@ To proceed you need to have the Docker desktop GUI app running (open).
----------------------
7. to run the image run "docker run --rm -it --net=host -e DISPLAY=$DISPLAY -v /tmp:/tmp ur_simple_control /bin/zsh".
[Linux and MAC]: NOTE: FIRST RUN "xhost +" EVERY TIME before you run the image. otherwise docker can't open new windows,
- so real-time-plotting won't work. [MAC] you need to have xquartz installed
+ so real-time-plotting won't work. [MAC] you need to have xquartz installed to have the xhost + command (look at installation for more).
+ NOTE! if you don't get real-time-plotting with xhost +, do xhost + 127.0.0.1 and replace -e DISPLAY=$DISPLAY with
+ -e DISPLAY=127.0.0.1:0 .
[WINDOWS]: NOTE: i've been told that mapping the /tmp folder is not necessary on windows and that it in fact might cause problems.
in this case just remove the "-v /tmp:/tmp" from the command
8. verify installation by running an example with --visualize-manipulator and --real-time-plotting arguments
diff --git a/python/convenience_tool_box/freedrive_v2.0.py b/python/convenience_tool_box/freedrive_v2.0.py
index fd41882..ea35610 100644
--- a/python/convenience_tool_box/freedrive_v2.0.py
+++ b/python/convenience_tool_box/freedrive_v2.0.py
@@ -5,6 +5,7 @@ import argparse
from functools import partial
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
from ur_simple_control.basics.basics import freedriveUntilKeyboard
+import pickle
def get_args():
parser = getMinimalArgParser()
@@ -18,6 +19,9 @@ if __name__ == "__main__":
robot = RobotManager(args)
pose_n_q_dict = freedriveUntilKeyboard(args, robot)
+ file = open("./pose_n_q_dict.pickle", 'wb')
+ pickle.dump(pose_n_q_dict, file)
+ file.close()
print(pose_n_q_dict)
# get expected behaviour here (library can't know what the end is - you have to do this here)
diff --git a/python/convenience_tool_box/open_close_gripper.py b/python/convenience_tool_box/open_close_gripper.py
index af5f97c..32d9e36 100644
--- a/python/convenience_tool_box/open_close_gripper.py
+++ b/python/convenience_tool_box/open_close_gripper.py
@@ -1,31 +1,35 @@
-from rtde_control import RTDEControlInterface as RTDEControl
-from rtde_receive import RTDEReceiveInterface as RTDEReceive
-from rtde_io import RTDEIOInterface
-from ur_simple_control.util.robotiq_gripper import RobotiqGripper
import pinocchio as pin
import numpy as np
-import os
import time
-import signal
-import matplotlib.pyplot as plt
+import argparse
+from functools import partial
+from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
+def get_args():
+ parser = getMinimalArgParser()
+ parser.description = 'open gripper, wait a few seconds, then close the gripper'
+ # add more arguments here from different Simple Manipulator Control modules
+ args = parser.parse_args()
+ return args
-def handler(signum, frame):
- gripper.move(255,255,255)
- time.sleep(2)
- exit()
+if __name__ == "__main__":
+ args = get_args()
+ robot = RobotManager(args)
-signal.signal(signal.SIGINT, handler)
+ robot.openGripper()
+ time.sleep(5)
+ robot.closeGripper()
-gripper = RobotiqGripper()
-gripper.connect("192.168.1.102", 63352)
-#gripper.connect("192.168.1.3", 63352)
-time.sleep(6)
-gripper.activate()
-#time.sleep(3)
-gripper.move(0,100,100)
-time.sleep(4)
-gripper.move(255,255,255)
-#time.sleep(300)
-input("press Enter to close and quit")
-handler(None, None)
+ # 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()
+ #loop_manager.stopHandler(None, None)
diff --git a/python/examples/casadi_ocp_collision_avoidance.py b/python/examples/casadi_ocp_collision_avoidance.py
new file mode 100644
index 0000000..cc84856
--- /dev/null
+++ b/python/examples/casadi_ocp_collision_avoidance.py
@@ -0,0 +1,34 @@
+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
+from ur_simple_control.optimal_control.get_ocp_args import get_OCP_args
+
+def get_args():
+ parser = getMinimalArgParser()
+ parser.description = 'optimal control in pinocchio.casadi for obstacle avoidance'
+ # 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)
+
+
+
+ # 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()
+ #loop_manager.stopHandler(None, None)
diff --git a/python/examples/crocoddyl_mpc.py b/python/examples/crocoddyl_mpc.py
index a8ac8f3..ce9bd05 100644
--- a/python/examples/crocoddyl_mpc.py
+++ b/python/examples/crocoddyl_mpc.py
@@ -3,7 +3,8 @@ import time
import argparse
from functools import partial
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
-from ur_simple_control.optimal_control.crocoddyl_optimal_control import get_OCP_args, createCrocoIKOCP, solveCrocoOCP
+from ur_simple_control.optimal_control.crocoddyl_optimal_control import createCrocoIKOCP, solveCrocoOCP
+from ur_simple_control.optimal_control.get_ocp_args import get_OCP_args
from ur_simple_control.optimal_control.crocoddyl_mpc import CrocoIKMPC
from ur_simple_control.basics.basics import followKinematicJointTrajP
from ur_simple_control.util.logging_utils import LogManager
diff --git a/python/examples/crocoddyl_ocp_clik.py b/python/examples/crocoddyl_ocp_clik.py
index 986ce88..1b7ece6 100644
--- a/python/examples/crocoddyl_ocp_clik.py
+++ b/python/examples/crocoddyl_ocp_clik.py
@@ -3,7 +3,8 @@ import time
import argparse
from functools import partial
from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
-from ur_simple_control.optimal_control.crocoddyl_optimal_control import get_OCP_args, createCrocoIKOCP, solveCrocoOCP
+from ur_simple_control.optimal_control.crocoddyl_optimal_control import createCrocoIKOCP, solveCrocoOCP
+from ur_simple_control.optimal_control.get_ocp_args import get_OCP_args
from ur_simple_control.basics.basics import followKinematicJointTrajP
from ur_simple_control.util.logging_utils import LogManager
from ur_simple_control.visualize.visualize import plotFromDict
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index 02d8bca..a0622a1 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -21,9 +21,6 @@ from ur_simple_control.basics.basics import moveJPI
#######################################################################
def getArgs():
- #######################################################################
- # generic arguments #
- #######################################################################
parser = getMinimalArgParser()
parser = getClikArgs(parser)
parser = getDMPArgs(parser)
@@ -60,7 +57,7 @@ def getArgs():
# go and pick up the marker
# marker position:
-# NOTE: this function does not really work (needs more points)
+# NOTE: this function does not work (needs more points)
"""
R =
-0.73032 -0.682357 0.0319574
@@ -125,16 +122,16 @@ def getMarker(args, robot, rotation_matrix, translation_vector):
#moveL(args, robot, Tinit.copy())
print("got back")
-"""
-findMarkerOffset
----------------
-This relies on having the correct orientation of the plane
-and the correct translation vector for top-left corner.
-Idea is you pick up the marker, go to the top corner,
-touch it, and see the difference between that and the translation vector.
-Obviously it's just a hacked solution, but it works so who cares.
-"""
def findMarkerOffset(args, robot, plane_pose):
+ """
+ findMarkerOffset
+ ---------------
+ This relies on having the correct orientation of the plane
+ and the correct translation vector for top-left corner.
+ Idea is you pick up the marker, go to the top corner,
+ touch it, and see the difference between that and the translation vector.
+ Obviously it's just a hacked solution, but it works so who cares.
+ """
above_starting_write_point = pin.SE3.Identity()
above_starting_write_point.translation[2] = -0.2
plane_pose = pin.SE3(rotation_matrix, translation_vector)
@@ -158,16 +155,7 @@ def findMarkerOffset(args, robot, plane_pose):
compliantMoveL(args, robot, above_starting_write_point)
return marker_offset
-#######################################################################
-# control loop #
-#######################################################################
-
-# feedforward velocity, feedback position and force for impedance
-# TODO: actually write this out
-def controller():
- pass
-
-def controlLoopWriting(args, robot : RobotManager, dmp, tc, controller, i, past_data):
+def controlLoopWriting(args, robot : RobotManager, dmp, tc, i, past_data):
"""
controlLoopWriting
-----------------------
@@ -182,7 +170,7 @@ def controlLoopWriting(args, robot : RobotManager, dmp, tc, controller, i, past_
dmp.set_tau(tau_dmp)
q = robot.getQ()
T_w_e = robot.getT_w_e()
- Z = np.diag(np.array([0.0, 0.0, 2.0, 1.0, 1.0, 1.0]))
+ Z = np.diag(np.array([0.0, 0.0, 1.0, 0.5, 0.5, 0.5]))
wrench = robot.getWrench()
save_past_dict['wrench'] = wrench.copy()
@@ -203,9 +191,7 @@ def controlLoopWriting(args, robot : RobotManager, dmp, tc, controller, i, past_
# compute control law:
# - feedforward the velocity and the force reading
# - feedback the position
- # TODO: don't use vel for qd, it's confusion (yes, that means changing dmp code too)
- # TODO: put this in a controller function for easy swapping (or don't if you won't swap)
- # solve this q[:6] bs (clean it up)
+ # TODO solve this q[:6] bs (clean it up)
vel_cmd = dmp.vel + args.kp * (dmp.pos - q[:6].reshape((6,1))) + args.alpha * tau
robot.sendQd(vel_cmd)
@@ -248,12 +234,10 @@ def write(args, robot : RobotManager, joint_trajectory):
first_q = np.array(first_q)
# move to initial pose
mtool = robot.getT_w_e(q_given=first_q)
- #mtool.translation[1] = mtool.translation[1] - 0.0035
- #mtool.translation[1] = mtool.translation[1] - 0.012
- # NOTE: magic number for how close to the board to get
- # it should be fine without it too,
- # this might just be an atavism which fixed something broken before
- mtool.translation[1] = mtool.translation[1] - 0.006
+ # start a bit above
+ go_away_from_plane_transf = pin.SE3.Identity()
+ go_away_from_plane_transf.translation[2] = -0.002
+ mtool = mtool.act(go_away_from_plane_transf)
if not args.board_wiping:
compliantMoveL(args, robot, mtool)
else:
@@ -272,7 +256,7 @@ def write(args, robot : RobotManager, joint_trajectory):
'tau' : np.zeros(robot.n_arm_joints),
}
#moveJ(args, robot, dmp.pos.reshape((6,)))
- controlLoop = partial(controlLoopWriting, args, robot, dmp, tc, controller)
+ controlLoop = partial(controlLoopWriting, args, robot, dmp, tc)
loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
# and now we can actually run
loop_manager.run()
@@ -316,7 +300,7 @@ if __name__ == "__main__":
args.n_calibration_tests)
print("finished calibration")
else:
- print("using predefined values")
+ print("using existing plane calibration")
file = open('./plane_pose.pickle', 'rb')
plane_calib_dict = pickle.load(file)
file.close()
@@ -338,6 +322,7 @@ if __name__ == "__main__":
if args.find_marker_offset:
# find the marker offset
marker_offset = findMarkerOffset(args, robot, plane_pose)
+ robot.sendQd(np.zeros(6))
print('marker_offset', marker_offset)
# we're going in a bit deeper
if not args.board_wiping:
@@ -350,34 +335,47 @@ if __name__ == "__main__":
else:
print("i hope you know the magic number of marker length + going into board")
#path = path + np.array([0.0, 0.0, -0.1503])
- path_points_3D = path_points_3D + np.array([0.0, 0.0, - 0.092792+ 0.003])
+ path_points_3D = path_points_3D + np.array([0.0, 0.0, - 0.192792+ 0.003])
# create a joint space trajectory based on the 3D path
if args.draw_new or args.calibration or args.find_marker_offset:
path = []
for i in range(len(path_points_3D)):
- # TODO: test
path_pose = pin.SE3.Identity()
path_pose.translation = path_points_3D[i]
path.append(plane_pose.act(path_pose))
+ print(
+ """
+ look at the viz now! we're constructing a trajectory for the drawing.
+ it has to look reasonable, otherwise we can't run it!
+ """
+ )
joint_trajectory = clikCartesianPathIntoJointPath(args, robot, path, \
clikController, q_init, plane_pose)
+ answer = input("did the movement of the manipulator look reasonable? [Y/n]")
+ if not (answer == "Y" or answer == "y"):
+ print("well if it doesn't look reasonable i'll just exit!")
+ answer = False
+ else:
+ answer = True
else:
joint_trajectory_file_path = './joint_trajectory.csv'
joint_trajectory = np.genfromtxt(joint_trajectory_file_path, delimiter=',')
- write(args, robot, joint_trajectory)
+ if answer:
+ write(args, robot, joint_trajectory)
if not args.pinocchio_only:
robot.stopRobot()
- if args.save_log:
- robot.log_manager.plotAllControlLoops()
-
if args.save_log:
robot.log_manager.saveLog()
if args.visualize_manipulator:
robot.killManipulatorVisualizer()
+ if args.save_log:
+ robot.log_manager.plotAllControlLoops()
+
+
diff --git a/python/examples/joint_trajectory.csv b/python/examples/joint_trajectory.csv
index 97892e5..54aba40 100644
--- a/python/examples/joint_trajectory.csv
+++ b/python/examples/joint_trajectory.csv
@@ -1,312 +1,355 @@
-0.00000,1.34590,-1.33594,-1.29510,-1.17724,1.75084,-0.14072
-0.01608,1.34159,-1.33577,-1.29548,-1.17667,1.75514,-0.14275
-0.03215,1.33709,-1.33611,-1.29547,-1.17620,1.75937,-0.14505
-0.04823,1.32423,-1.33884,-1.29400,-1.17530,1.77067,-0.15221
-0.06431,1.31543,-1.34128,-1.29251,-1.17488,1.77818,-0.15730
-0.08039,1.30795,-1.34349,-1.29125,-1.17453,1.78445,-0.16172
-0.09646,1.30107,-1.34576,-1.28984,-1.17430,1.79014,-0.16586
-0.11254,1.28205,-1.35248,-1.28596,-1.17368,1.80551,-0.17758
-0.12862,1.27491,-1.35492,-1.28473,-1.17339,1.81124,-0.18204
-0.14469,1.26023,-1.35986,-1.28263,-1.17263,1.82290,-0.19130
-0.16077,1.25317,-1.36218,-1.28177,-1.17223,1.82850,-0.19580
-0.17685,1.24586,-1.36487,-1.28048,-1.17197,1.83427,-0.20048
-0.19293,1.24225,-1.36626,-1.27973,-1.17190,1.83711,-0.20280
-0.20900,1.23684,-1.36847,-1.27844,-1.17186,1.84137,-0.20628
-0.22508,1.23142,-1.37080,-1.27698,-1.17189,1.84563,-0.20979
-0.24116,1.21891,-1.37634,-1.27346,-1.17202,1.85540,-0.21792
-0.25723,1.21477,-1.37826,-1.27216,-1.17214,1.85864,-0.22062
-0.27331,1.21001,-1.38040,-1.27078,-1.17224,1.86236,-0.22374
-0.28939,1.20533,-1.38259,-1.26930,-1.17239,1.86600,-0.22681
-0.30547,1.20075,-1.38480,-1.26775,-1.17258,1.86958,-0.22983
-0.32154,1.19668,-1.38681,-1.26630,-1.17279,1.87274,-0.23251
-0.33762,1.19210,-1.38914,-1.26458,-1.17306,1.87630,-0.23554
-0.35370,1.18814,-1.39119,-1.26303,-1.17333,1.87938,-0.23817
-0.36977,1.18420,-1.39328,-1.26143,-1.17362,1.88243,-0.24079
-0.38585,1.17969,-1.39583,-1.25932,-1.17407,1.88593,-0.24379
-0.40193,1.17526,-1.39836,-1.25721,-1.17454,1.88936,-0.24675
-0.41801,1.16867,-1.40218,-1.25403,-1.17526,1.89446,-0.25115
-0.43408,1.16756,-1.40283,-1.25348,-1.17539,1.89532,-0.25190
-0.45016,1.15106,-1.41442,-1.24218,-1.17858,1.90804,-0.26301
-0.46624,1.14644,-1.41766,-1.23899,-1.17955,1.91160,-0.26615
-0.48232,1.12804,-1.43093,-1.22595,-1.18348,1.92573,-0.27870
-0.49839,1.10622,-1.44721,-1.20974,-1.18864,1.94242,-0.29376
-0.51447,1.09031,-1.45936,-1.19752,-1.19273,1.95456,-0.30488
-0.53055,1.08222,-1.46554,-1.19130,-1.19489,1.96072,-0.31057
-0.54662,1.06092,-1.48124,-1.17639,-1.19985,1.97686,-0.32569
-0.56270,1.05295,-1.48708,-1.17085,-1.20180,1.98289,-0.33141
-0.57878,1.03476,-1.50012,-1.15912,-1.20575,1.99658,-0.34455
-0.59486,1.00663,-1.51969,-1.14270,-1.21119,2.01759,-0.36516
-0.61093,1.00031,-1.52427,-1.13863,-1.21270,2.02232,-0.36987
-0.62701,0.98879,-1.53295,-1.13086,-1.21553,2.03089,-0.37849
-0.64309,0.98620,-1.53499,-1.12895,-1.21626,2.03282,-0.38045
-0.65916,0.98145,-1.53882,-1.12532,-1.21765,2.03635,-0.38403
-0.67524,0.97888,-1.54104,-1.12308,-1.21852,2.03826,-0.38598
-0.69132,0.97804,-1.54177,-1.12235,-1.21881,2.03887,-0.38662
-0.70740,0.96816,-1.55120,-1.11238,-1.22269,2.04617,-0.39412
-0.72347,0.96335,-1.55587,-1.10738,-1.22470,2.04973,-0.39781
-0.73955,0.93624,-1.58432,-1.07616,-1.23709,2.06957,-0.41875
-0.75563,0.91973,-1.60169,-1.05703,-1.24505,2.08159,-0.43177
-0.77170,0.91602,-1.60575,-1.05239,-1.24708,2.08429,-0.43473
-0.78778,0.90376,-1.61842,-1.03877,-1.25283,2.09316,-0.44454
-0.80386,0.89814,-1.62402,-1.03288,-1.25538,2.09721,-0.44908
-0.81994,0.89217,-1.62999,-1.02665,-1.25806,2.10151,-0.45392
-0.83601,0.87900,-1.64249,-1.01447,-1.26317,2.11093,-0.46465
-0.85209,0.87408,-1.64711,-1.00997,-1.26512,2.11444,-0.46870
-0.86817,0.86697,-1.65382,-1.00354,-1.26788,2.11951,-0.47459
-0.88424,0.86247,-1.65798,-0.99962,-1.26958,2.12270,-0.47832
-0.90032,0.85648,-1.66352,-0.99448,-1.27178,2.12694,-0.48332
-0.91640,0.85291,-1.66658,-0.99186,-1.27292,2.12947,-0.48631
-0.93248,0.84329,-1.67346,-0.98746,-1.27474,2.13624,-0.49440
-0.94855,0.83925,-1.67610,-0.98606,-1.27534,2.13907,-0.49782
-0.96463,0.83255,-1.68001,-0.98468,-1.27588,2.14376,-0.50351
-0.98071,0.82612,-1.68366,-0.98363,-1.27626,2.14825,-0.50901
-0.99678,0.82132,-1.68630,-0.98304,-1.27645,2.15159,-0.51313
-1.01286,0.81780,-1.68819,-0.98272,-1.27655,2.15403,-0.51616
-1.02894,0.81397,-1.69007,-0.98271,-1.27650,2.15669,-0.51947
-1.04502,0.81035,-1.69151,-0.98332,-1.27619,2.15920,-0.52262
-1.06109,0.81005,-1.69164,-0.98336,-1.27617,2.15940,-0.52288
-1.07717,0.80253,-1.69403,-0.98584,-1.27495,2.16458,-0.52942
-1.09325,0.79362,-1.69606,-0.99038,-1.27277,2.17070,-0.53723
-1.10932,0.77332,-1.69478,-1.01200,-1.26273,2.18450,-0.55523
-1.12540,0.76826,-1.69395,-1.01844,-1.25974,2.18793,-0.55978
-1.14148,0.75504,-1.69079,-1.03734,-1.25095,2.19682,-0.57171
-1.15756,0.74132,-1.68557,-1.06081,-1.24006,2.20597,-0.58423
-1.17363,0.73275,-1.68133,-1.07744,-1.23235,2.21165,-0.59213
-1.18971,0.72231,-1.67464,-1.10060,-1.22168,2.21851,-0.60181
-1.20579,0.71612,-1.67012,-1.11544,-1.21483,2.22257,-0.60759
-1.22186,0.71252,-1.66712,-1.12477,-1.21053,2.22492,-0.61097
-1.23794,0.70935,-1.66445,-1.13308,-1.20669,2.22698,-0.61394
-1.25402,0.70459,-1.66003,-1.14625,-1.20066,2.23007,-0.61844
-1.27010,0.69914,-1.65458,-1.16207,-1.19344,2.23359,-0.62360
-1.28617,0.68943,-1.64413,-1.19158,-1.18008,2.23982,-0.63286
-1.30225,0.68593,-1.64008,-1.20281,-1.17495,2.24205,-0.63621
-1.31833,0.67948,-1.63232,-1.22397,-1.16539,2.24616,-0.64242
-1.33441,0.67324,-1.62432,-1.24534,-1.15578,2.25011,-0.64846
-1.35048,0.67113,-1.62146,-1.25289,-1.15235,2.25144,-0.65051
-1.36656,0.66937,-1.61901,-1.25929,-1.14946,2.25255,-0.65221
-1.38264,0.66525,-1.61303,-1.27464,-1.14260,2.25514,-0.65624
-1.39871,0.66030,-1.60523,-1.29412,-1.13398,2.25824,-0.66109
-1.41479,0.65705,-1.60018,-1.30683,-1.12832,2.26026,-0.66428
-1.43087,0.65382,-1.59506,-1.31961,-1.12266,2.26227,-0.66746
-1.44695,0.64890,-1.58621,-1.34082,-1.11340,2.26531,-0.67232
-1.46302,0.64540,-1.57941,-1.35680,-1.10642,2.26748,-0.67580
-1.47910,0.64316,-1.57467,-1.36770,-1.10166,2.26885,-0.67802
-1.49518,0.64155,-1.57103,-1.37595,-1.09807,2.26984,-0.67962
-1.51125,0.64000,-1.56717,-1.38448,-1.09438,2.27079,-0.68116
-1.52733,0.63747,-1.55937,-1.40092,-1.08736,2.27235,-0.68368
-1.54341,0.63581,-1.55178,-1.41590,-1.08101,2.27336,-0.68533
-1.55949,0.63493,-1.54478,-1.42894,-1.07552,2.27389,-0.68617
-1.57556,0.63316,-1.52769,-1.46010,-1.06257,2.27495,-0.68789
-1.59164,0.63221,-1.51349,-1.48539,-1.05204,2.27551,-0.68876
-1.60772,0.63216,-1.50879,-1.49351,-1.04862,2.27553,-0.68877
-1.62379,0.63269,-1.49662,-1.51348,-1.04041,2.27519,-0.68815
-1.63987,0.63461,-1.47987,-1.53939,-1.02992,2.27399,-0.68608
-1.65595,0.63543,-1.47356,-1.54913,-1.02591,2.27347,-0.68518
-1.67203,0.63932,-1.44529,-1.59159,-1.00910,2.27102,-0.68105
-1.68810,0.64420,-1.42186,-1.62482,-0.99603,2.26797,-0.67594
-1.70418,0.64618,-1.41472,-1.63452,-0.99214,2.26673,-0.67388
-1.72026,0.65299,-1.39679,-1.65637,-0.98377,2.26248,-0.66691
-1.73633,0.65714,-1.38706,-1.66787,-0.97932,2.25987,-0.66268
-1.75241,0.66338,-1.37441,-1.68179,-0.97407,2.25594,-0.65638
-1.76849,0.66825,-1.36548,-1.69119,-0.97053,2.25286,-0.65150
-1.78457,0.67009,-1.36229,-1.69448,-0.96926,2.25169,-0.64965
-1.80064,0.67675,-1.35170,-1.70469,-0.96552,2.24747,-0.64305
-1.81672,0.68418,-1.33854,-1.71823,-0.96060,2.24271,-0.63571
-1.83280,0.68759,-1.33207,-1.72523,-0.95801,2.24051,-0.63235
-1.84887,0.70951,-1.28849,-1.77196,-0.94200,2.22631,-0.61116
-1.86495,0.72178,-1.26753,-1.79329,-0.93459,2.21826,-0.59946
-1.88103,0.74630,-1.23254,-1.82477,-0.92457,2.20204,-0.57658
-1.89711,0.75264,-1.22413,-1.83234,-0.92200,2.19778,-0.57073
-1.91318,0.76787,-1.20425,-1.84954,-0.91669,2.18753,-0.55687
-1.92926,0.77264,-1.19835,-1.85458,-0.91506,2.18429,-0.55255
-1.94534,0.78785,-1.17995,-1.86965,-0.91072,2.17395,-0.53899
-1.96141,0.79083,-1.17659,-1.87235,-0.90989,2.17190,-0.53633
-1.97749,0.80471,-1.16147,-1.88378,-0.90682,2.16239,-0.52417
-1.99357,0.81927,-1.14714,-1.89364,-0.90439,2.15233,-0.51156
-2.00965,0.83295,-1.13495,-1.90122,-0.90269,2.14281,-0.49987
-2.02572,0.83955,-1.12940,-1.90452,-0.90195,2.13820,-0.49428
-2.04180,0.84876,-1.12252,-1.90789,-0.90137,2.13174,-0.48654
-2.05788,0.86004,-1.11456,-1.91138,-0.90093,2.12380,-0.47715
-2.07395,0.86745,-1.10973,-1.91319,-0.90077,2.11855,-0.47103
-2.09003,0.87339,-1.10617,-1.91425,-0.90077,2.11434,-0.46615
-2.10611,0.89191,-1.09652,-1.91542,-0.90165,2.10117,-0.45112
-2.12219,0.90239,-1.09156,-1.91560,-0.90225,2.09365,-0.44271
-2.13826,0.90662,-1.08953,-1.91576,-0.90245,2.09060,-0.43933
-2.15434,0.91547,-1.08570,-1.91552,-0.90311,2.08422,-0.43231
-2.17042,0.92992,-1.07962,-1.91489,-0.90437,2.07377,-0.42096
-2.18650,0.94218,-1.07489,-1.91391,-0.90558,2.06486,-0.41143
-2.20257,0.96523,-1.06839,-1.90891,-0.90895,2.04802,-0.39377
-2.21865,0.98232,-1.06456,-1.90417,-0.91175,2.03543,-0.38087
-2.23473,0.99294,-1.06227,-1.90123,-0.91351,2.02757,-0.37293
-2.25080,0.99879,-1.06116,-1.89945,-0.91452,2.02323,-0.36858
-2.26688,1.00699,-1.05969,-1.89685,-0.91601,2.01713,-0.36252
-2.28296,1.02140,-1.05808,-1.89093,-0.91909,2.00640,-0.35196
-2.29904,1.05607,-1.05879,-1.87052,-0.92872,1.98044,-0.32699
-2.31511,1.07392,-1.06069,-1.85824,-0.93422,1.96694,-0.31434
-2.33119,1.10160,-1.06934,-1.83120,-0.94547,1.94592,-0.29503
-2.34727,1.11093,-1.07307,-1.82102,-0.94964,1.93878,-0.28858
-2.36334,1.12426,-1.07956,-1.80469,-0.95630,1.92858,-0.27945
-2.37942,1.13314,-1.08465,-1.79269,-0.96117,1.92176,-0.27341
-2.39550,1.13905,-1.08837,-1.78419,-0.96461,1.91722,-0.26941
-2.41158,1.14452,-1.09230,-1.77558,-0.96807,1.91301,-0.26571
-2.42765,1.14900,-1.09604,-1.76778,-0.97116,1.90956,-0.26270
-2.44373,1.15034,-1.09746,-1.76498,-0.97223,1.90852,-0.26180
-2.45981,1.15489,-1.10840,-1.74649,-0.97879,1.90497,-0.25875
-2.47588,1.15687,-1.11706,-1.73251,-0.98368,1.90341,-0.25743
-2.49196,1.15834,-1.13231,-1.70866,-0.99197,1.90224,-0.25649
-2.50804,1.15864,-1.13773,-1.70016,-0.99499,1.90201,-0.25631
-2.52412,1.15882,-1.14716,-1.68549,-1.00020,1.90186,-0.25622
-2.54019,1.15859,-1.15378,-1.67529,-1.00384,1.90204,-0.25640
-2.55627,1.15725,-1.16292,-1.66172,-1.00856,1.90306,-0.25733
-2.57235,1.15578,-1.17032,-1.65085,-1.01236,1.90418,-0.25835
-2.58842,1.15426,-1.17568,-1.64318,-1.01501,1.90535,-0.25939
-2.60450,1.14787,-1.19345,-1.61843,-1.02338,1.91024,-0.26376
-2.62058,1.14427,-1.20277,-1.60538,-1.02791,1.91301,-0.26623
-2.63666,1.14068,-1.21143,-1.59333,-1.03212,1.91577,-0.26870
-2.65273,1.13717,-1.21982,-1.58161,-1.03626,1.91847,-0.27112
-2.66881,1.12678,-1.24254,-1.55035,-1.04718,1.92644,-0.27829
-2.68489,1.12348,-1.24899,-1.54151,-1.05036,1.92898,-0.28058
-2.70096,1.11189,-1.27164,-1.51060,-1.06140,1.93786,-0.28864
-2.71704,1.10629,-1.28244,-1.49568,-1.06691,1.94216,-0.29257
-2.73312,1.10326,-1.28834,-1.48747,-1.06999,1.94448,-0.29471
-2.74920,1.09566,-1.30314,-1.46696,-1.07762,1.95029,-0.30005
-2.76527,1.09163,-1.31041,-1.45698,-1.08138,1.95337,-0.30291
-2.78135,1.08757,-1.31712,-1.44796,-1.08475,1.95647,-0.30578
-2.79743,1.07968,-1.32958,-1.43148,-1.09085,1.96246,-0.31137
-2.81350,1.07274,-1.34038,-1.41720,-1.09619,1.96774,-0.31631
-2.82958,1.05816,-1.36517,-1.38370,-1.10888,1.97879,-0.32676
-2.84566,1.04961,-1.37884,-1.36531,-1.11604,1.98525,-0.33295
-2.86174,1.04497,-1.38606,-1.35562,-1.11986,1.98877,-0.33633
-2.87781,1.03278,-1.40496,-1.33052,-1.12963,1.99794,-0.34522
-2.89389,1.03000,-1.40918,-1.32487,-1.13191,2.00004,-0.34727
-2.90997,1.02342,-1.41899,-1.31190,-1.13707,2.00498,-0.35211
-2.92605,1.01811,-1.42653,-1.30207,-1.14099,2.00896,-0.35603
-2.94212,1.01382,-1.43240,-1.29454,-1.14399,2.01218,-0.35921
-2.95820,1.00366,-1.45144,-1.26781,-1.15486,2.01979,-0.36680
-2.97428,0.99961,-1.45814,-1.25861,-1.15868,2.02281,-0.36985
-2.99035,0.97719,-1.48617,-1.22450,-1.17213,2.03940,-0.38671
-3.00643,0.96953,-1.49530,-1.21346,-1.17668,2.04507,-0.39257
-3.02251,0.95589,-1.51079,-1.19549,-1.18394,2.05511,-0.40304
-3.03859,0.95101,-1.51620,-1.18923,-1.18654,2.05869,-0.40682
-3.05466,0.93337,-1.53508,-1.16846,-1.19487,2.07158,-0.42056
-3.07074,0.91409,-1.55597,-1.14557,-1.20418,2.08557,-0.43579
-3.08682,0.90819,-1.56250,-1.13823,-1.20731,2.08985,-0.44053
-3.10289,0.88996,-1.58155,-1.11825,-1.21553,2.10296,-0.45521
-3.11897,0.86951,-1.60113,-1.09943,-1.22328,2.11756,-0.47195
-3.13505,0.86651,-1.60407,-1.09649,-1.22458,2.11970,-0.47445
-3.15113,0.85661,-1.61184,-1.09052,-1.22710,2.12672,-0.48269
-3.16720,0.85011,-1.61647,-1.08748,-1.22839,2.13131,-0.48814
-3.18328,0.83699,-1.62384,-1.08526,-1.22916,2.14052,-0.49921
-3.19936,0.82860,-1.62765,-1.08555,-1.22893,2.14638,-0.50635
-3.21543,0.82456,-1.62937,-1.08593,-1.22873,2.14920,-0.50982
-3.23151,0.81654,-1.63176,-1.08862,-1.22744,2.15477,-0.51672
-3.24759,0.81045,-1.63300,-1.09178,-1.22597,2.15899,-0.52199
-3.26367,0.80267,-1.63317,-1.09838,-1.22298,2.16435,-0.52876
-3.27974,0.79058,-1.63048,-1.11422,-1.21584,2.17264,-0.53937
-3.29582,0.78380,-1.62780,-1.12530,-1.21085,2.17727,-0.54535
-3.31190,0.77217,-1.62003,-1.15019,-1.19967,2.18516,-0.55569
-3.32797,0.76619,-1.61470,-1.16551,-1.19277,2.18920,-0.56103
-3.34405,0.76225,-1.61068,-1.17658,-1.18776,2.19186,-0.56456
-3.36013,0.74988,-1.59306,-1.22016,-1.16833,2.20013,-0.57570
-3.37621,0.74722,-1.58829,-1.23139,-1.16325,2.20190,-0.57808
-3.39228,0.74482,-1.58312,-1.24305,-1.15802,2.20350,-0.58025
-3.40836,0.74009,-1.57069,-1.26986,-1.14616,2.20663,-0.58451
-3.42444,0.73420,-1.54185,-1.32645,-1.12158,2.21049,-0.58978
-3.44051,0.73352,-1.53636,-1.33692,-1.11691,2.21094,-0.59035
-3.45659,0.73387,-1.51686,-1.37061,-1.10245,2.21070,-0.58990
-3.47267,0.73432,-1.50718,-1.38713,-1.09529,2.21038,-0.58939
-3.48875,0.73504,-1.49447,-1.40855,-1.08612,2.20989,-0.58863
-3.50482,0.74032,-1.45974,-1.46276,-1.06366,2.20635,-0.58355
-3.52090,0.74553,-1.44067,-1.49035,-1.05223,2.20286,-0.57862
-3.53698,0.74772,-1.43270,-1.50193,-1.04737,2.20138,-0.57653
-3.55305,0.75715,-1.39971,-1.54840,-1.02880,2.19503,-0.56775
-3.56913,0.76064,-1.38902,-1.56325,-1.02273,2.19266,-0.56449
-3.58521,0.77143,-1.36289,-1.59665,-1.00980,2.18536,-0.55465
-3.60129,0.78200,-1.34275,-1.62039,-1.00077,2.17818,-0.54512
-3.61736,0.78711,-1.33387,-1.63057,-0.99686,2.17468,-0.54053
-3.63344,0.80577,-1.30483,-1.66151,-0.98582,2.16189,-0.52409
-3.64952,0.80983,-1.29892,-1.66780,-0.98345,2.15907,-0.52053
-3.66559,0.82188,-1.28115,-1.68646,-0.97687,2.15071,-0.51008
-3.68167,0.82511,-1.27653,-1.69135,-0.97509,2.14845,-0.50729
-3.69775,0.84297,-1.25091,-1.71767,-0.96627,2.13597,-0.49208
-3.71383,0.85322,-1.23789,-1.73037,-0.96204,2.12875,-0.48346
-3.72990,0.86131,-1.22855,-1.73895,-0.95926,2.12303,-0.47671
-3.74598,0.86643,-1.22305,-1.74378,-0.95772,2.11940,-0.47245
-3.76206,0.87939,-1.21061,-1.75355,-0.95494,2.11021,-0.46182
-3.77814,0.88533,-1.20536,-1.75745,-0.95384,2.10596,-0.45697
-3.79421,0.90371,-1.19315,-1.76318,-0.95292,2.09284,-0.44219
-3.81029,0.91331,-1.18808,-1.76443,-0.95299,2.08592,-0.43454
-3.82637,0.93480,-1.18069,-1.76132,-0.95534,2.07038,-0.41766
-3.84244,0.95225,-1.17748,-1.75498,-0.95855,2.05767,-0.40417
-3.85852,0.96453,-1.17655,-1.74869,-0.96145,2.04866,-0.39478
-3.87460,0.97479,-1.17646,-1.74246,-0.96425,2.04110,-0.38700
-3.89068,1.00055,-1.18224,-1.71776,-0.97470,2.02206,-0.36777
-3.90675,1.01125,-1.18628,-1.70512,-0.97992,2.01407,-0.35988
-3.92283,1.02443,-1.19453,-1.68451,-0.98824,2.00422,-0.35027
-3.93891,1.04450,-1.21478,-1.64078,-1.00580,1.98918,-0.33583
-3.95498,1.05217,-1.22507,-1.61985,-1.01428,1.98339,-0.33038
-3.97106,1.05589,-1.23158,-1.60723,-1.01937,1.98058,-0.32776
-3.98714,1.05915,-1.24111,-1.59001,-1.02617,1.97810,-0.32547
-4.00322,1.05903,-1.26292,-1.55492,-1.03946,1.97815,-0.32562
-4.01929,1.05433,-1.28951,-1.51413,-1.05498,1.98167,-0.32907
-4.03537,1.05033,-1.30388,-1.49259,-1.06331,1.98469,-0.33200
-4.05145,1.04659,-1.31468,-1.47673,-1.06947,1.98750,-0.33474
-4.06752,1.03355,-1.34330,-1.43670,-1.08468,1.99728,-0.34424
-4.08360,1.02085,-1.36684,-1.40462,-1.09706,2.00681,-0.35359
-4.09968,1.01444,-1.37729,-1.39069,-1.10254,2.01160,-0.35834
-4.11576,0.99251,-1.40565,-1.35643,-1.11543,2.02790,-0.37466
-4.13183,0.98399,-1.41518,-1.34549,-1.11967,2.03422,-0.38108
-4.14791,0.97018,-1.42898,-1.33091,-1.12514,2.04441,-0.39155
-4.16399,0.94610,-1.45113,-1.30941,-1.13297,2.06207,-0.41004
-4.18006,0.93756,-1.45832,-1.30287,-1.13547,2.06831,-0.41669
-4.19614,0.92106,-1.47183,-1.29135,-1.13966,2.08031,-0.42965
-4.21222,0.91341,-1.47755,-1.28697,-1.14129,2.08586,-0.43572
-4.22830,0.89429,-1.49071,-1.27856,-1.14409,2.09962,-0.45102
-4.24437,0.88914,-1.49414,-1.27648,-1.14485,2.10332,-0.45519
-4.26045,0.87587,-1.50178,-1.27352,-1.14566,2.11279,-0.46600
-4.27653,0.87219,-1.50379,-1.27288,-1.14585,2.11541,-0.46902
-4.29260,0.86406,-1.50764,-1.27262,-1.14573,2.12119,-0.47572
-4.30868,0.85615,-1.51069,-1.27365,-1.14508,2.12678,-0.48229
-4.32476,0.84808,-1.51305,-1.27612,-1.14382,2.13248,-0.48903
-4.34084,0.83990,-1.51427,-1.28074,-1.14166,2.13822,-0.49591
-4.35691,0.83374,-1.51440,-1.28562,-1.13946,2.14253,-0.50112
-4.37299,0.82706,-1.51371,-1.29242,-1.13644,2.14720,-0.50680
-4.38907,0.81761,-1.51075,-1.30561,-1.13065,2.15377,-0.51489
-4.40514,0.80908,-1.50612,-1.32103,-1.12394,2.15966,-0.52223
-4.42122,0.79888,-1.49638,-1.34682,-1.11283,2.16668,-0.53107
-4.43730,0.79110,-1.48209,-1.37832,-1.09945,2.17202,-0.53784
-4.45338,0.78724,-1.47074,-1.40130,-1.08971,2.17466,-0.54119
-4.46945,0.78569,-1.46093,-1.41950,-1.08207,2.17572,-0.54251
-4.48553,0.78942,-1.43301,-1.46330,-1.06433,2.17320,-0.53914
-4.50161,0.79174,-1.42199,-1.47999,-1.05745,2.17161,-0.53702
-4.51768,0.80062,-1.39919,-1.51045,-1.04540,2.16554,-0.52915
-4.53376,0.82638,-1.35909,-1.55500,-1.02872,2.14777,-0.50683
-4.54984,0.83494,-1.34790,-1.56675,-1.02418,2.14179,-0.49948
-4.56592,0.84919,-1.33091,-1.58339,-1.01810,2.13179,-0.48742
-4.58199,0.86029,-1.31833,-1.59542,-1.01378,2.12396,-0.47813
-4.59807,0.86959,-1.30742,-1.60618,-1.00994,2.11736,-0.47041
-4.61415,0.88812,-1.28542,-1.62785,-1.00258,2.10415,-0.45525
-4.63023,0.90184,-1.27291,-1.63808,-0.99932,2.09432,-0.44417
-4.64630,0.90534,-1.27016,-1.64007,-0.99869,2.09179,-0.44135
-4.66238,0.90890,-1.26777,-1.64143,-0.99831,2.08923,-0.43851
-4.67846,0.91491,-1.26482,-1.64203,-0.99830,2.08490,-0.43373
-4.69453,0.92271,-1.26240,-1.64062,-0.99911,2.07926,-0.42756
-4.71061,0.94054,-1.26427,-1.62592,-1.00520,2.06632,-0.41362
-4.72669,0.95222,-1.26990,-1.60948,-1.01172,2.05777,-0.40459
-4.74277,0.96250,-1.28244,-1.58293,-1.02205,2.05019,-0.39673
-4.75884,0.97012,-1.30320,-1.54440,-1.03714,2.04455,-0.39098
-4.77492,0.97216,-1.31397,-1.52534,-1.04474,2.04303,-0.38948
-4.79100,0.97014,-1.34796,-1.47035,-1.06640,2.04450,-0.39112
-4.80707,0.96862,-1.35988,-1.45113,-1.07427,2.04563,-0.39234
-4.82315,0.96195,-1.38844,-1.40717,-1.09201,2.05054,-0.39755
-4.83923,0.95683,-1.40250,-1.38648,-1.10047,2.05431,-0.40153
-4.85531,0.94773,-1.41812,-1.36599,-1.10862,2.06095,-0.40858
-4.87138,0.92991,-1.43959,-1.34182,-1.11789,2.07390,-0.42248
-4.88746,0.92248,-1.44714,-1.33404,-1.12095,2.07929,-0.42834
-4.90354,0.91802,-1.45114,-1.33030,-1.12241,2.08252,-0.43187
-4.91961,0.90944,-1.45702,-1.32640,-1.12380,2.08871,-0.43869
-4.93569,0.90301,-1.46051,-1.32508,-1.12419,2.09333,-0.44384
-4.95177,0.89730,-1.46288,-1.32521,-1.12400,2.09743,-0.44843
-4.96785,0.88930,-1.46472,-1.32800,-1.12265,2.10317,-0.45489
-4.98392,0.87499,-1.46402,-1.34011,-1.11725,2.11336,-0.46653
-5.00000,0.87496,-1.46402,-1.34014,-1.11724,2.11338,-0.46656
+0.00000,1.31458,-1.32887,-1.36187,-1.12090,1.77566,-0.15965
+0.01412,1.31427,-1.32875,-1.36211,-1.12073,1.77600,-0.15978
+0.02825,1.31319,-1.32822,-1.36314,-1.12008,1.77717,-0.16023
+0.04237,1.31119,-1.32692,-1.36563,-1.11865,1.77929,-0.16109
+0.05650,1.30567,-1.32131,-1.37625,-1.11329,1.78488,-0.16367
+0.07062,1.29821,-1.31016,-1.39722,-1.10346,1.79195,-0.16747
+0.08475,1.29492,-1.30442,-1.40804,-1.09846,1.79497,-0.16923
+0.09887,1.29222,-1.29917,-1.41793,-1.09396,1.79737,-0.17071
+0.11299,1.28799,-1.28971,-1.43562,-1.08606,1.80100,-0.17312
+0.12712,1.28387,-1.27952,-1.45463,-1.07766,1.80442,-0.17554
+0.14124,1.28074,-1.27134,-1.46985,-1.07097,1.80696,-0.17740
+0.15537,1.27873,-1.26591,-1.47995,-1.06654,1.80858,-0.17861
+0.16949,1.27625,-1.25913,-1.49252,-1.06106,1.81055,-0.18012
+0.18362,1.27234,-1.24832,-1.51250,-1.05243,1.81363,-0.18252
+0.19774,1.26875,-1.23983,-1.52843,-1.04549,1.81643,-0.18473
+0.21186,1.26571,-1.23346,-1.54055,-1.04017,1.81880,-0.18663
+0.22599,1.26253,-1.22642,-1.55381,-1.03442,1.82128,-0.18861
+0.24011,1.25274,-1.20557,-1.59288,-1.01772,1.82886,-0.19475
+0.25424,1.24919,-1.19883,-1.60574,-1.01213,1.83162,-0.19698
+0.26836,1.24507,-1.19164,-1.61959,-1.00612,1.83481,-0.19959
+0.28249,1.23813,-1.17974,-1.64238,-0.99634,1.84018,-0.20399
+0.29661,1.23450,-1.17318,-1.65483,-0.99104,1.84299,-0.20629
+0.31073,1.23127,-1.16794,-1.66493,-0.98670,1.84549,-0.20836
+0.32486,1.22332,-1.15520,-1.68935,-0.97639,1.85163,-0.21344
+0.33898,1.21695,-1.14551,-1.70803,-0.96850,1.85655,-0.21753
+0.35311,1.21259,-1.13940,-1.71998,-0.96342,1.85992,-0.22034
+0.36723,1.20806,-1.13342,-1.73176,-0.95842,1.86341,-0.22326
+0.38136,1.20047,-1.12446,-1.74978,-0.95075,1.86926,-0.22818
+0.39548,1.19037,-1.11309,-1.77278,-0.94103,1.87703,-0.23477
+0.40960,1.18520,-1.10754,-1.78413,-0.93622,1.88101,-0.23815
+0.42373,1.17637,-1.09866,-1.80251,-0.92845,1.88779,-0.24395
+0.43785,1.16808,-1.09102,-1.81868,-0.92160,1.89416,-0.24942
+0.45198,1.16238,-1.08584,-1.82965,-0.91697,1.89854,-0.25321
+0.46610,1.15325,-1.07798,-1.84652,-0.90988,1.90552,-0.25928
+0.48023,1.14054,-1.06696,-1.87004,-0.90016,1.91524,-0.26780
+0.49435,1.13557,-1.06296,-1.87878,-0.89652,1.91905,-0.27115
+0.50847,1.13001,-1.05857,-1.88840,-0.89253,1.92329,-0.27490
+0.52260,1.12594,-1.05556,-1.89516,-0.88971,1.92640,-0.27766
+0.53672,1.12372,-1.05385,-1.89893,-0.88816,1.92809,-0.27917
+0.55085,1.12262,-1.05298,-1.90083,-0.88738,1.92893,-0.27992
+0.56497,1.12206,-1.05255,-1.90178,-0.88699,1.92936,-0.28029
+0.57910,1.12154,-1.05213,-1.90269,-0.88662,1.92975,-0.28065
+0.59322,1.11992,-1.05078,-1.90558,-0.88545,1.93099,-0.28175
+0.60734,1.11992,-1.05078,-1.90559,-0.88545,1.93099,-0.28175
+0.62147,1.11993,-1.05078,-1.90558,-0.88545,1.93099,-0.28175
+0.63559,1.12656,-1.05579,-1.89457,-0.88976,1.92607,-0.27725
+0.64972,1.13229,-1.06079,-1.88407,-0.89385,1.92179,-0.27338
+0.66384,1.13765,-1.06605,-1.87337,-0.89800,1.91774,-0.26977
+0.67797,1.14074,-1.07031,-1.86542,-0.90098,1.91540,-0.26769
+0.69209,1.14284,-1.07371,-1.85923,-0.90327,1.91380,-0.26628
+0.70621,1.14570,-1.07899,-1.84984,-0.90674,1.91162,-0.26436
+0.72034,1.14746,-1.08342,-1.84228,-0.90948,1.91027,-0.26319
+0.73446,1.14882,-1.08813,-1.83449,-0.91225,1.90923,-0.26229
+0.74859,1.15022,-1.09430,-1.82443,-0.91582,1.90815,-0.26136
+0.76271,1.15102,-1.09933,-1.81638,-0.91867,1.90754,-0.26084
+0.77684,1.15144,-1.10382,-1.80933,-0.92114,1.90721,-0.26057
+0.79096,1.15212,-1.11106,-1.79789,-0.92520,1.90669,-0.26014
+0.80508,1.15239,-1.11632,-1.78965,-0.92812,1.90648,-0.25997
+0.81921,1.15244,-1.12097,-1.78243,-0.93068,1.90644,-0.25996
+0.83333,1.15256,-1.12774,-1.77185,-0.93446,1.90636,-0.25991
+0.84746,1.15223,-1.13351,-1.76305,-0.93758,1.90661,-0.26015
+0.86158,1.15188,-1.13905,-1.75455,-0.94061,1.90689,-0.26041
+0.87571,1.15150,-1.14460,-1.74604,-0.94367,1.90718,-0.26069
+0.88983,1.15080,-1.14987,-1.73812,-0.94648,1.90773,-0.26119
+0.90395,1.14985,-1.15458,-1.73119,-0.94890,1.90846,-0.26185
+0.91808,1.14911,-1.15756,-1.72687,-0.95041,1.90903,-0.26236
+0.93220,1.14862,-1.15945,-1.72414,-0.95137,1.90940,-0.26269
+0.94633,1.14743,-1.16298,-1.71918,-0.95305,1.91031,-0.26351
+0.96045,1.14577,-1.16661,-1.71435,-0.95463,1.91158,-0.26464
+0.97458,1.14296,-1.17076,-1.70932,-0.95614,1.91372,-0.26655
+0.98870,1.13943,-1.17461,-1.70517,-0.95724,1.91640,-0.26894
+1.00282,1.13559,-1.17796,-1.70200,-0.95794,1.91932,-0.27155
+1.01695,1.12921,-1.18243,-1.69847,-0.95849,1.92417,-0.27589
+1.03107,1.11998,-1.18770,-1.69528,-0.95860,1.93118,-0.28220
+1.04520,1.11103,-1.19201,-1.69347,-0.95827,1.93797,-0.28835
+1.05932,1.09781,-1.19708,-1.69297,-0.95700,1.94801,-0.29750
+1.07345,1.08318,-1.20134,-1.69471,-0.95479,1.95909,-0.30770
+1.08757,1.07519,-1.20327,-1.69632,-0.95337,1.96513,-0.31332
+1.10169,1.06494,-1.20515,-1.69942,-0.95117,1.97289,-0.32057
+1.11582,1.05480,-1.20661,-1.70323,-0.94872,1.98053,-0.32777
+1.12994,1.04419,-1.20767,-1.70806,-0.94584,1.98851,-0.33537
+1.14407,1.03245,-1.20816,-1.71462,-0.94222,1.99732,-0.34383
+1.15819,1.02458,-1.20814,-1.71963,-0.93958,2.00322,-0.34954
+1.17232,1.01755,-1.20766,-1.72487,-0.93696,2.00847,-0.35466
+1.18644,1.01072,-1.20681,-1.73060,-0.93418,2.01357,-0.35967
+1.20056,1.00734,-1.20633,-1.73355,-0.93276,2.01608,-0.36215
+1.21469,1.00266,-1.20540,-1.73806,-0.93065,2.01957,-0.36560
+1.22881,0.99564,-1.20372,-1.74535,-0.92728,2.02478,-0.37079
+1.24294,0.98893,-1.20228,-1.75212,-0.92412,2.02976,-0.37578
+1.25706,0.98377,-1.20091,-1.75775,-0.92154,2.03357,-0.37962
+1.27119,0.98030,-1.20015,-1.76130,-0.91989,2.03613,-0.38221
+1.28531,0.97487,-1.19923,-1.76650,-0.91741,2.04013,-0.38628
+1.29944,0.97235,-1.19882,-1.76890,-0.91627,2.04198,-0.38818
+1.31356,0.96934,-1.19845,-1.77158,-0.91497,2.04420,-0.39045
+1.32768,0.96373,-1.19820,-1.77594,-0.91277,2.04831,-0.39469
+1.34181,0.95922,-1.19842,-1.77882,-0.91121,2.05161,-0.39810
+1.35593,0.95434,-1.19899,-1.78144,-0.90970,2.05517,-0.40182
+1.37006,0.95069,-1.19960,-1.78313,-0.90868,2.05784,-0.40460
+1.38418,0.94519,-1.20098,-1.78500,-0.90737,2.06184,-0.40881
+1.39831,0.94049,-1.20265,-1.78585,-0.90651,2.06526,-0.41243
+1.41243,0.93311,-1.20607,-1.78598,-0.90561,2.07061,-0.41813
+1.42655,0.92533,-1.21036,-1.78508,-0.90506,2.07625,-0.42418
+1.44068,0.91878,-1.21453,-1.78347,-0.90494,2.08099,-0.42931
+1.45480,0.91035,-1.22095,-1.77975,-0.90542,2.08708,-0.43596
+1.46893,0.90291,-1.22721,-1.77551,-0.90625,2.09245,-0.44187
+1.48305,0.89199,-1.23827,-1.76638,-0.90861,2.10030,-0.45062
+1.49718,0.88618,-1.24479,-1.76042,-0.91037,2.10448,-0.45532
+1.51130,0.87981,-1.25278,-1.75255,-0.91284,2.10906,-0.46051
+1.52542,0.87378,-1.26132,-1.74348,-0.91585,2.11339,-0.46545
+1.53955,0.86829,-1.26961,-1.73436,-0.91896,2.11731,-0.46997
+1.55367,0.86235,-1.27925,-1.72340,-0.92280,2.12156,-0.47490
+1.56780,0.85729,-1.28883,-1.71176,-0.92703,2.12518,-0.47913
+1.58192,0.85078,-1.30510,-1.69024,-0.93510,2.12983,-0.48462
+1.59605,0.84846,-1.31289,-1.67908,-0.93949,2.13151,-0.48662
+1.61017,0.84688,-1.32175,-1.66547,-0.94497,2.13268,-0.48801
+1.62429,0.84610,-1.32763,-1.65614,-0.94878,2.13326,-0.48871
+1.63842,0.84616,-1.33685,-1.64040,-0.95531,2.13328,-0.48874
+1.65254,0.84672,-1.34102,-1.63275,-0.95857,2.13292,-0.48833
+1.66667,0.84818,-1.34614,-1.62265,-0.96294,2.13194,-0.48717
+1.68079,0.85047,-1.35154,-1.61132,-0.96789,2.13037,-0.48532
+1.69492,0.85277,-1.35541,-1.60261,-0.97176,2.12879,-0.48347
+1.70904,0.85552,-1.35909,-1.59383,-0.97570,2.12689,-0.48124
+1.72316,0.85778,-1.36167,-1.58739,-0.97862,2.12532,-0.47941
+1.73729,0.85952,-1.36318,-1.58322,-0.98054,2.12411,-0.47800
+1.75141,0.86049,-1.36393,-1.58107,-0.98154,2.12343,-0.47721
+1.76554,0.86095,-1.36429,-1.58004,-0.98202,2.12311,-0.47684
+1.77966,0.86208,-1.36495,-1.57788,-0.98304,2.12233,-0.47593
+1.79379,0.86210,-1.36497,-1.57783,-0.98307,2.12231,-0.47591
+1.80791,0.86246,-1.36509,-1.57729,-0.98334,2.12206,-0.47561
+1.82203,0.86315,-1.36520,-1.57646,-0.98376,2.12158,-0.47505
+1.83616,0.86505,-1.36507,-1.57493,-0.98465,2.12025,-0.47352
+1.85028,0.86508,-1.36507,-1.57491,-0.98466,2.12023,-0.47349
+1.86441,0.86691,-1.36438,-1.57436,-0.98515,2.11895,-0.47201
+1.87853,0.86910,-1.36312,-1.57445,-0.98544,2.11741,-0.47024
+1.89266,0.87241,-1.36064,-1.57556,-0.98547,2.11509,-0.46757
+1.90678,0.87787,-1.35513,-1.57983,-0.98454,2.11123,-0.46317
+1.92090,0.88226,-1.34958,-1.58522,-0.98297,2.10811,-0.45962
+1.93503,0.88520,-1.34509,-1.59018,-0.98136,2.10601,-0.45726
+1.94915,0.88788,-1.34040,-1.59575,-0.97945,2.10408,-0.45509
+1.96328,0.89044,-1.33505,-1.60254,-0.97700,2.10223,-0.45302
+1.97740,0.89247,-1.33054,-1.60841,-0.97485,2.10077,-0.45137
+1.99153,0.89421,-1.32570,-1.61510,-0.97233,2.09950,-0.44996
+2.00565,0.89638,-1.31592,-1.62971,-0.96666,2.09789,-0.44816
+2.01977,0.89641,-1.30684,-1.64497,-0.96048,2.09781,-0.44808
+2.03390,0.89520,-1.30024,-1.65708,-0.95544,2.09861,-0.44897
+2.04802,0.89388,-1.29650,-1.66449,-0.95227,2.09953,-0.44999
+2.06215,0.89231,-1.29348,-1.67090,-0.94950,2.10062,-0.45121
+2.07627,0.88988,-1.29040,-1.67811,-0.94631,2.10231,-0.45312
+2.09040,0.88749,-1.28787,-1.68437,-0.94353,2.10399,-0.45501
+2.10452,0.88514,-1.28602,-1.68944,-0.94123,2.10564,-0.45687
+2.11864,0.87751,-1.28422,-1.69903,-0.93652,2.11100,-0.46298
+2.13277,0.87051,-1.28439,-1.70484,-0.93340,2.11592,-0.46862
+2.14689,0.86091,-1.28618,-1.71028,-0.93016,2.12266,-0.47644
+2.16102,0.85370,-1.28833,-1.71306,-0.92829,2.12772,-0.48237
+2.17514,0.83998,-1.29438,-1.71523,-0.92600,2.13730,-0.49375
+2.18927,0.83555,-1.29651,-1.71564,-0.92543,2.14040,-0.49747
+2.20339,0.83157,-1.29863,-1.71568,-0.92505,2.14318,-0.50082
+2.21751,0.82885,-1.30022,-1.71547,-0.92488,2.14508,-0.50313
+2.23164,0.82517,-1.30260,-1.71484,-0.92479,2.14764,-0.50625
+2.24576,0.81962,-1.30725,-1.71222,-0.92529,2.15149,-0.51097
+2.25989,0.81243,-1.31571,-1.70486,-0.92747,2.15649,-0.51714
+2.27401,0.80750,-1.32348,-1.69654,-0.93031,2.15993,-0.52141
+2.28814,0.80319,-1.33229,-1.68586,-0.93420,2.16294,-0.52518
+2.30226,0.80103,-1.33804,-1.67820,-0.93713,2.16446,-0.52709
+2.31638,0.79821,-1.35101,-1.65904,-0.94468,2.16648,-0.52963
+2.33051,0.79704,-1.36090,-1.64335,-0.95105,2.16735,-0.53073
+2.34463,0.79630,-1.37121,-1.62649,-0.95798,2.16791,-0.53145
+2.35876,0.79577,-1.38093,-1.61038,-0.96465,2.16832,-0.53199
+2.37288,0.79562,-1.38999,-1.59498,-0.97109,2.16847,-0.53220
+2.38701,0.79557,-1.39844,-1.58049,-0.97719,2.16854,-0.53231
+2.40113,0.79565,-1.40411,-1.57062,-0.98138,2.16851,-0.53229
+2.41525,0.79595,-1.41027,-1.55966,-0.98606,2.16833,-0.53208
+2.42938,0.79655,-1.41698,-1.54745,-0.99130,2.16795,-0.53161
+2.44350,0.79730,-1.42150,-1.53885,-0.99504,2.16746,-0.53101
+2.45763,0.79795,-1.42481,-1.53244,-0.99785,2.16703,-0.53047
+2.47175,0.79857,-1.42715,-1.52773,-0.99993,2.16661,-0.52995
+2.48588,0.79892,-1.42830,-1.52537,-1.00098,2.16639,-0.52967
+2.50000,0.79892,-1.42833,-1.52531,-1.00101,2.16638,-0.52966
+2.51412,0.79909,-1.42887,-1.52421,-1.00150,2.16627,-0.52952
+2.52825,0.79928,-1.42945,-1.52299,-1.00204,2.16614,-0.52936
+2.54237,0.79928,-1.42946,-1.52297,-1.00205,2.16614,-0.52937
+2.55650,0.79928,-1.42947,-1.52296,-1.00206,2.16614,-0.52937
+2.57062,0.79927,-1.42946,-1.52298,-1.00205,2.16615,-0.52938
+2.58475,0.79829,-1.42820,-1.52583,-1.00101,2.16681,-0.53025
+2.59887,0.79214,-1.41980,-1.54498,-0.99352,2.17096,-0.53569
+2.61299,0.78919,-1.41608,-1.55381,-0.98993,2.17296,-0.53829
+2.62712,0.78703,-1.41318,-1.56062,-0.98714,2.17441,-0.54019
+2.64124,0.78333,-1.40872,-1.57151,-0.98258,2.17690,-0.54345
+2.65537,0.77870,-1.40367,-1.58438,-0.97711,2.18002,-0.54753
+2.66949,0.77526,-1.40085,-1.59246,-0.97359,2.18234,-0.55057
+2.68362,0.77207,-1.39854,-1.59945,-0.97052,2.18448,-0.55339
+2.69774,0.76938,-1.39726,-1.60427,-0.96834,2.18628,-0.55577
+2.71186,0.76556,-1.39626,-1.60977,-0.96578,2.18884,-0.55917
+2.72599,0.76147,-1.39603,-1.61429,-0.96358,2.19157,-0.56281
+2.74011,0.75744,-1.39643,-1.61773,-0.96181,2.19426,-0.56643
+2.75424,0.74975,-1.39836,-1.62238,-0.95923,2.19937,-0.57337
+2.76836,0.74545,-1.39987,-1.62427,-0.95809,2.20223,-0.57727
+2.78249,0.74267,-1.40097,-1.62528,-0.95744,2.20407,-0.57981
+2.79661,0.74082,-1.40187,-1.62569,-0.95712,2.20530,-0.58150
+2.81073,0.73742,-1.40394,-1.62577,-0.95678,2.20755,-0.58462
+2.82486,0.73304,-1.40714,-1.62499,-0.95671,2.21044,-0.58865
+2.83898,0.72798,-1.41160,-1.62286,-0.95713,2.21378,-0.59333
+2.85311,0.72186,-1.41848,-1.61779,-0.95865,2.21781,-0.59903
+2.86723,0.71721,-1.42495,-1.61184,-0.96070,2.22088,-0.60341
+2.88136,0.71112,-1.43586,-1.59989,-0.96513,2.22489,-0.60917
+2.89548,0.70610,-1.44931,-1.58235,-0.97206,2.22822,-0.61400
+2.90960,0.70338,-1.46003,-1.56681,-0.97846,2.23004,-0.61666
+2.92373,0.70091,-1.48971,-1.51812,-0.99889,2.23178,-0.61922
+2.93785,0.70083,-1.49936,-1.50109,-1.00639,2.23189,-0.61940
+2.95198,0.70108,-1.50661,-1.48790,-1.01223,2.23176,-0.61925
+2.96610,0.70141,-1.51107,-1.47957,-1.01596,2.23157,-0.61898
+2.98023,0.70191,-1.51546,-1.47115,-1.01972,2.23127,-0.61856
+2.99435,0.70259,-1.52086,-1.46077,-1.02434,2.23086,-0.61797
+3.00847,0.70297,-1.52407,-1.45461,-1.02710,2.23063,-0.61765
+3.02260,0.70377,-1.52898,-1.44497,-1.03141,2.23013,-0.61693
+3.03672,0.70444,-1.53226,-1.43834,-1.03439,2.22971,-0.61633
+3.05085,0.70642,-1.53911,-1.42388,-1.04089,2.22846,-0.61452
+3.06497,0.70718,-1.54136,-1.41899,-1.04312,2.22798,-0.61383
+3.07910,0.70805,-1.54354,-1.41408,-1.04537,2.22742,-0.61303
+3.09322,0.70875,-1.54513,-1.41043,-1.04705,2.22698,-0.61239
+3.10734,0.70978,-1.54721,-1.40553,-1.04931,2.22633,-0.61145
+3.12147,0.71003,-1.54772,-1.40431,-1.04987,2.22616,-0.61121
+3.13559,0.71056,-1.54873,-1.40189,-1.05099,2.22582,-0.61073
+3.14972,0.71083,-1.54924,-1.40067,-1.05155,2.22565,-0.61048
+3.16384,0.71111,-1.54974,-1.39946,-1.05211,2.22548,-0.61023
+3.17797,0.71138,-1.55023,-1.39825,-1.05267,2.22530,-0.60997
+3.19209,0.71140,-1.55026,-1.39819,-1.05270,2.22529,-0.60996
+3.20621,0.71080,-1.54999,-1.39922,-1.05230,2.22567,-0.61054
+3.22034,0.69281,-1.53790,-1.43887,-1.03564,2.23716,-0.62780
+3.23446,0.67926,-1.52760,-1.47161,-1.02141,2.24577,-0.64093
+3.24859,0.67718,-1.52596,-1.47680,-1.01912,2.24709,-0.64295
+3.26271,0.66976,-1.52019,-1.49522,-1.01100,2.25177,-0.65020
+3.27684,0.66361,-1.51546,-1.51047,-1.00426,2.25562,-0.65624
+3.29096,0.66007,-1.51241,-1.51984,-1.00014,2.25784,-0.65974
+3.30508,0.65440,-1.50687,-1.53593,-0.99313,2.26138,-0.66536
+3.31921,0.64956,-1.50218,-1.54964,-0.98716,2.26438,-0.67018
+3.33333,0.64547,-1.49819,-1.56129,-0.98208,2.26691,-0.67427
+3.34746,0.64152,-1.49392,-1.57325,-0.97692,2.26934,-0.67823
+3.36158,0.63696,-1.48881,-1.58735,-0.97087,2.27214,-0.68283
+3.37571,0.63439,-1.48478,-1.59723,-0.96669,2.27371,-0.68544
+3.38983,0.63152,-1.47937,-1.60973,-0.96145,2.27546,-0.68834
+3.40395,0.62927,-1.47369,-1.62195,-0.95639,2.27683,-0.69062
+3.41808,0.62768,-1.46750,-1.63419,-0.95138,2.27780,-0.69223
+3.43220,0.62665,-1.46026,-1.64747,-0.94600,2.27842,-0.69326
+3.44633,0.62629,-1.45512,-1.65648,-0.94236,2.27864,-0.69361
+3.46045,0.62652,-1.44729,-1.66919,-0.93730,2.27850,-0.69334
+3.47458,0.62853,-1.43459,-1.68778,-0.93006,2.27727,-0.69124
+3.48870,0.63145,-1.42238,-1.70451,-0.92357,2.27548,-0.68819
+3.50282,0.63707,-1.40299,-1.72984,-0.91390,2.27201,-0.68236
+3.51695,0.64269,-1.38561,-1.75194,-0.90548,2.26851,-0.67655
+3.53107,0.64843,-1.36933,-1.77208,-0.89786,2.26492,-0.67065
+3.54520,0.65298,-1.35758,-1.78619,-0.89254,2.26207,-0.66600
+3.55932,0.65603,-1.35017,-1.79494,-0.88924,2.26014,-0.66289
+3.57345,0.66393,-1.33402,-1.81230,-0.88301,2.25517,-0.65493
+3.58757,0.67285,-1.31734,-1.82947,-0.87696,2.24952,-0.64604
+3.60169,0.67765,-1.30941,-1.83717,-0.87423,2.24646,-0.64128
+3.61582,0.68564,-1.29736,-1.84804,-0.87055,2.24135,-0.63343
+3.62994,0.69426,-1.28527,-1.85839,-0.86713,2.23581,-0.62503
+3.64407,0.70248,-1.27404,-1.86787,-0.86403,2.23050,-0.61709
+3.65819,0.71103,-1.26292,-1.87690,-0.86116,2.22493,-0.60891
+3.67232,0.71749,-1.25483,-1.88332,-0.85913,2.22070,-0.60276
+3.68644,0.74384,-1.22335,-1.90677,-0.85262,2.20333,-0.57818
+3.70056,0.75587,-1.21030,-1.91605,-0.84996,2.19528,-0.56714
+3.71469,0.76552,-1.20032,-1.92285,-0.84809,2.18878,-0.55837
+3.72881,0.78272,-1.18334,-1.93367,-0.84548,2.17713,-0.54297
+3.74294,0.79738,-1.16938,-1.94241,-0.84346,2.16711,-0.53003
+3.75706,0.80594,-1.16136,-1.94749,-0.84227,2.16122,-0.52255
+3.77119,0.81038,-1.15738,-1.94992,-0.84171,2.15815,-0.51869
+3.78531,0.82048,-1.14841,-1.95520,-0.84065,2.15116,-0.50998
+3.79944,0.82971,-1.14043,-1.95979,-0.83979,2.14474,-0.50210
+3.81356,0.85132,-1.12253,-1.96930,-0.83853,2.12963,-0.48390
+3.82768,0.86152,-1.11467,-1.97325,-0.83803,2.12242,-0.47542
+3.84181,0.86802,-1.10979,-1.97566,-0.83774,2.11782,-0.47006
+3.85593,0.87580,-1.10399,-1.97848,-0.83746,2.11229,-0.46367
+3.87006,0.89724,-1.08833,-1.98568,-0.83722,2.09700,-0.44633
+3.88418,0.90983,-1.07961,-1.98954,-0.83714,2.08795,-0.43628
+3.89831,0.92103,-1.07190,-1.99300,-0.83710,2.07986,-0.42742
+3.91243,0.92994,-1.06607,-1.99541,-0.83719,2.07340,-0.42043
+3.92655,0.93737,-1.06126,-1.99738,-0.83730,2.06800,-0.41464
+3.94068,0.94693,-1.05548,-1.99937,-0.83764,2.06103,-0.40724
+3.95480,0.96057,-1.04800,-2.00116,-0.83851,2.05106,-0.39678
+3.96893,0.97122,-1.04262,-2.00205,-0.83935,2.04324,-0.38868
+3.98305,0.98065,-1.03822,-2.00240,-0.84024,2.03630,-0.38158
+3.99718,0.98971,-1.03413,-2.00261,-0.84114,2.02960,-0.37479
+4.01130,1.00704,-1.02695,-2.00213,-0.84328,2.01677,-0.36194
+4.02542,1.01695,-1.02310,-2.00165,-0.84453,2.00938,-0.35466
+4.03955,1.02677,-1.01989,-2.00043,-0.84600,2.00205,-0.34749
+4.05367,1.03748,-1.01705,-1.99824,-0.84787,1.99404,-0.33973
+4.06780,1.04414,-1.01564,-1.99646,-0.84913,1.98903,-0.33493
+4.08192,1.05354,-1.01417,-1.99325,-0.85114,1.98196,-0.32820
+4.09605,1.05924,-1.01340,-1.99117,-0.85241,1.97766,-0.32413
+4.11017,1.07392,-1.01223,-1.98466,-0.85609,1.96658,-0.31374
+4.12429,1.07934,-1.01208,-1.98196,-0.85752,1.96247,-0.30992
+4.13842,1.08294,-1.01220,-1.97986,-0.85854,1.95973,-0.30739
+4.15254,1.08814,-1.01287,-1.97618,-0.86020,1.95577,-0.30375
+4.16667,1.09236,-1.01372,-1.97278,-0.86167,1.95255,-0.30080
+4.18079,1.09871,-1.01612,-1.96613,-0.86431,1.94770,-0.29638
+4.19492,1.10299,-1.01912,-1.95978,-0.86658,1.94441,-0.29340
+4.20904,1.10587,-1.02196,-1.95437,-0.86843,1.94218,-0.29141
+4.22316,1.10718,-1.02633,-1.94775,-0.87036,1.94114,-0.29049
+4.23729,1.10565,-1.03482,-1.93697,-0.87301,1.94220,-0.29152
+4.25141,1.10095,-1.04451,-1.92612,-0.87531,1.94566,-0.29474
+4.26554,1.09636,-1.05131,-1.91913,-0.87666,1.94908,-0.29791
+4.27966,1.08624,-1.06263,-1.90876,-0.87828,1.95666,-0.30494
+4.29379,1.07525,-1.07270,-1.90055,-0.87930,1.96491,-0.31263
+4.30791,1.06270,-1.08294,-1.89294,-0.88006,1.97434,-0.32150
+4.32203,1.05216,-1.09102,-1.88726,-0.88058,1.98225,-0.32901
+4.33616,1.04007,-1.09993,-1.88130,-0.88108,1.99132,-0.33768
+4.35028,1.02390,-1.11158,-1.87381,-0.88168,2.00341,-0.34939
+4.36441,1.00247,-1.12664,-1.86467,-0.88236,2.01937,-0.36512
+4.37853,0.98677,-1.13787,-1.85772,-0.88312,2.03101,-0.37681
+4.39266,0.96692,-1.15318,-1.84748,-0.88466,2.04567,-0.39177
+4.40678,0.94099,-1.17435,-1.83272,-0.88735,2.06467,-0.41166
+4.42090,0.92302,-1.18979,-1.82135,-0.88989,2.07777,-0.42569
+4.43503,0.90453,-1.20606,-1.80932,-0.89274,2.09116,-0.44035
+4.44915,0.88499,-1.22453,-1.79486,-0.89652,2.10520,-0.45607
+4.46328,0.87203,-1.23697,-1.78499,-0.89930,2.11447,-0.46666
+4.47740,0.85167,-1.25692,-1.76931,-0.90372,2.12890,-0.48351
+4.49153,0.83370,-1.27469,-1.75541,-0.90784,2.14154,-0.49863
+4.50565,0.82171,-1.28673,-1.74588,-0.91083,2.14992,-0.50887
+4.51977,0.79376,-1.31510,-1.72420,-0.91741,2.16922,-0.53311
+4.53390,0.78323,-1.32592,-1.71571,-0.92031,2.17646,-0.54245
+4.54802,0.76554,-1.34367,-1.70271,-0.92456,2.18848,-0.55829
+4.56215,0.74870,-1.36145,-1.68920,-0.92915,2.19982,-0.57363
+4.57627,0.73972,-1.37123,-1.68149,-0.93192,2.20583,-0.58193
+4.59040,0.72862,-1.38381,-1.67137,-0.93556,2.21321,-0.59228
+4.60452,0.71880,-1.39562,-1.66140,-0.93922,2.21970,-0.60153
+4.61864,0.71203,-1.40379,-1.65449,-0.94183,2.22415,-0.60797
+4.63277,0.70692,-1.41016,-1.64896,-0.94394,2.22750,-0.61286
+4.64689,0.70331,-1.41475,-1.64490,-0.94550,2.22986,-0.61633
+4.66102,0.69896,-1.42021,-1.64017,-0.94732,2.23269,-0.62052
+4.67514,0.69403,-1.42661,-1.63452,-0.94949,2.23589,-0.62530
+4.68927,0.68718,-1.43580,-1.62625,-0.95265,2.24032,-0.63197
+4.70339,0.68047,-1.44508,-1.61772,-0.95594,2.24463,-0.63855
+4.71751,0.67521,-1.45251,-1.61077,-0.95868,2.24800,-0.64375
+4.73164,0.67260,-1.45623,-1.60724,-0.96010,2.24967,-0.64634
+4.74576,0.67004,-1.45992,-1.60372,-0.96151,2.25130,-0.64888
+4.75989,0.66684,-1.46460,-1.59924,-0.96330,2.25334,-0.65208
+4.77401,0.66370,-1.46925,-1.59477,-0.96509,2.25533,-0.65522
+4.78814,0.65935,-1.47579,-1.58843,-0.96762,2.25809,-0.65959
+4.80226,0.65384,-1.48422,-1.58022,-0.97090,2.26156,-0.66515
+4.81638,0.64901,-1.49173,-1.57282,-0.97388,2.26459,-0.67005
+4.83051,0.64426,-1.49924,-1.56535,-0.97692,2.26756,-0.67490
+4.84463,0.64249,-1.50205,-1.56252,-0.97809,2.26867,-0.67671
+4.85876,0.64014,-1.50581,-1.55873,-0.97966,2.27013,-0.67912
+4.87288,0.63957,-1.50674,-1.55779,-0.98006,2.27049,-0.67972
+4.88701,0.63899,-1.50766,-1.55685,-0.98045,2.27085,-0.68031
+4.90113,0.63782,-1.50955,-1.55493,-0.98125,2.27157,-0.68152
+4.91525,0.63437,-1.51521,-1.54919,-0.98361,2.27372,-0.68508
+4.92938,0.62927,-1.52368,-1.54058,-0.98713,2.27687,-0.69036
+4.94350,0.62758,-1.52650,-1.53767,-0.98836,2.27791,-0.69212
+4.95763,0.62703,-1.52743,-1.53670,-0.98876,2.27825,-0.69269
+4.97175,0.62646,-1.52839,-1.53570,-0.98919,2.27860,-0.69330
+4.98588,0.62590,-1.52932,-1.53474,-0.98960,2.27894,-0.69387
+5.00000,0.62588,-1.52936,-1.53470,-0.98962,2.27896,-0.69390
diff --git a/python/examples/path_in_pixels.csv b/python/examples/path_in_pixels.csv
index dea19bc..5437a62 100644
--- a/python/examples/path_in_pixels.csv
+++ b/python/examples/path_in_pixels.csv
@@ -1,312 +1,355 @@
-0.07124,0.92223
-0.08401,0.92223
-0.09207,0.92223
-0.10887,0.92223
-0.11895,0.92223
-0.12702,0.92097
-0.13441,0.92097
-0.15457,0.91846
-0.16196,0.91595
-0.17742,0.91218
-0.18481,0.90967
-0.19288,0.90967
-0.19691,0.90967
-0.20296,0.90967
-0.20901,0.90967
-0.22312,0.90841
-0.22782,0.90841
-0.23320,0.90716
-0.23858,0.90716
-0.24395,0.90716
-0.24866,0.90716
-0.25403,0.90716
-0.25874,0.90716
-0.26344,0.90716
-0.26882,0.90841
-0.27419,0.90841
-0.28226,0.90841
-0.28360,0.90841
-0.30444,0.91469
-0.31048,0.91469
-0.33468,0.91720
-0.36425,0.91972
-0.38642,0.92097
-0.39785,0.92097
-0.42809,0.91972
-0.43952,0.91846
-0.46573,0.91595
-0.50672,0.90967
-0.51613,0.90841
-0.53360,0.90716
-0.53763,0.90716
-0.54503,0.90716
-0.54906,0.90841
-0.55040,0.90841
-0.56653,0.91092
-0.57460,0.91092
-0.62164,0.91469
-0.65121,0.91344
-0.65793,0.91344
-0.68011,0.91092
-0.69019,0.90841
-0.70094,0.90716
-0.72446,0.90213
-0.73320,0.89962
-0.74597,0.89711
-0.75403,0.89460
-0.76478,0.89208
-0.77083,0.88832
-0.78629,0.87827
-0.79234,0.87324
-0.80242,0.86571
-0.81250,0.85943
-0.81989,0.85440
-0.82527,0.85064
-0.83065,0.84561
-0.83468,0.83933
-0.83535,0.83933
-0.84476,0.82803
-0.85551,0.81421
-0.87366,0.77528
-0.87769,0.76523
-0.88978,0.73885
-0.89987,0.70871
-0.90524,0.68861
-0.90995,0.66224
-0.91263,0.64591
-0.91331,0.63586
-0.91465,0.62707
-0.91532,0.61325
-0.91599,0.59693
-0.91667,0.56678
-0.91667,0.55548
-0.91599,0.53413
-0.91465,0.51277
-0.91398,0.50524
-0.91331,0.49896
-0.91129,0.48389
-0.90793,0.46505
-0.90659,0.45249
-0.90457,0.43993
-0.89919,0.41983
-0.89516,0.40476
-0.89180,0.39471
-0.88911,0.38717
-0.88575,0.37964
-0.87836,0.36582
-0.86962,0.35452
-0.86089,0.34573
-0.84409,0.32186
-0.82930,0.30302
-0.82325,0.29800
-0.80847,0.28544
-0.78763,0.27037
-0.78024,0.26409
-0.74731,0.23645
-0.71774,0.21761
-0.70766,0.21385
-0.68212,0.20757
-0.66868,0.20380
-0.65054,0.20129
-0.63777,0.20003
-0.63306,0.20003
-0.61761,0.20003
-0.60013,0.19249
-0.59207,0.18747
-0.53562,0.16486
-0.50739,0.15733
-0.45901,0.15105
-0.44758,0.14853
-0.42070,0.14351
-0.41263,0.14225
-0.38777,0.13849
-0.38306,0.13849
-0.36223,0.13723
-0.34207,0.13849
-0.32460,0.14100
-0.31653,0.14225
-0.30645,0.14728
-0.29435,0.15105
-0.28696,0.15481
-0.28159,0.15858
-0.26546,0.16989
-0.25672,0.17617
-0.25269,0.17742
-0.24597,0.18370
-0.23387,0.19124
-0.22446,0.19877
-0.20968,0.21761
-0.19892,0.23143
-0.19153,0.23897
-0.18817,0.24399
-0.18280,0.25027
-0.17540,0.26409
-0.15860,0.30177
-0.15121,0.32312
-0.14315,0.36457
-0.14113,0.37964
-0.13710,0.40225
-0.13508,0.41857
-0.13374,0.42988
-0.13306,0.44118
-0.13306,0.45123
-0.13508,0.45500
-0.14315,0.47635
-0.14852,0.49142
-0.15659,0.51654
-0.15927,0.52533
-0.16398,0.54041
-0.16801,0.55045
-0.17540,0.56301
-0.18078,0.57306
-0.18616,0.57934
-0.20094,0.60069
-0.20833,0.61200
-0.21573,0.62205
-0.22245,0.63209
-0.24261,0.65721
-0.24933,0.66349
-0.27016,0.68736
-0.28024,0.69866
-0.28562,0.70494
-0.29973,0.72001
-0.30780,0.72629
-0.31586,0.73132
-0.32997,0.74137
-0.34207,0.75016
-0.36761,0.77276
-0.38374,0.78281
-0.39247,0.78784
-0.41465,0.80165
-0.42003,0.80416
-0.43212,0.81044
-0.44220,0.81421
-0.45027,0.81672
-0.46774,0.83682
-0.47715,0.83933
-0.51949,0.84561
-0.53360,0.84687
-0.55780,0.84938
-0.56653,0.84938
-0.59745,0.85064
-0.63172,0.85189
-0.64247,0.85189
-0.67540,0.84938
-0.71169,0.84184
-0.71707,0.84059
-0.73320,0.83054
-0.74328,0.82426
-0.76210,0.80919
-0.77285,0.79788
-0.77823,0.79286
-0.78696,0.78030
-0.79301,0.77025
-0.79839,0.75518
-0.80511,0.73006
-0.80780,0.71499
-0.81048,0.68610
-0.81048,0.66977
-0.81048,0.65847
-0.80578,0.61702
-0.80309,0.60697
-0.79973,0.59693
-0.79234,0.57432
-0.76949,0.53036
-0.76411,0.52282
-0.74261,0.50147
-0.73320,0.49017
-0.72110,0.47509
-0.68280,0.44118
-0.65860,0.42737
-0.64987,0.41983
-0.61223,0.39094
-0.59946,0.38215
-0.56586,0.36582
-0.53831,0.35703
-0.52621,0.35326
-0.48656,0.34196
-0.47849,0.33945
-0.45497,0.33065
-0.44892,0.32814
-0.41532,0.31558
-0.39718,0.31181
-0.38374,0.31056
-0.37567,0.31056
-0.35685,0.31181
-0.34879,0.31307
-0.32796,0.32563
-0.31922,0.33442
-0.30309,0.35577
-0.29301,0.37587
-0.28696,0.39094
-0.28159,0.40350
-0.27285,0.44369
-0.27083,0.46253
-0.27016,0.49017
-0.27083,0.54292
-0.27285,0.56678
-0.27554,0.58060
-0.28159,0.59818
-0.30040,0.62958
-0.32258,0.66349
-0.33602,0.67982
-0.34677,0.69113
-0.37702,0.71750
-0.40390,0.73634
-0.41734,0.74262
-0.45766,0.75392
-0.47312,0.75392
-0.49597,0.75392
-0.53427,0.75267
-0.54772,0.74890
-0.57325,0.74513
-0.58468,0.74011
-0.61290,0.73006
-0.62030,0.72629
-0.63844,0.71499
-0.64315,0.71122
-0.65323,0.70243
-0.66196,0.69238
-0.67003,0.68108
-0.67608,0.66726
-0.67944,0.65596
-0.68280,0.64340
-0.68548,0.62330
-0.68616,0.60321
-0.68280,0.57432
-0.67204,0.54417
-0.66263,0.52408
-0.65188,0.51026
-0.61761,0.48389
-0.60484,0.47384
-0.57392,0.46128
-0.51546,0.45123
-0.49866,0.44997
-0.47312,0.44746
-0.45430,0.44495
-0.43884,0.43993
-0.40793,0.43113
-0.38710,0.43616
-0.38239,0.43993
-0.37836,0.44495
-0.37366,0.45500
-0.36962,0.46756
-0.36828,0.50147
-0.37097,0.52659
-0.37970,0.55799
-0.39315,0.59693
-0.40121,0.61451
-0.43011,0.65973
-0.44019,0.67480
-0.46707,0.70620
-0.48387,0.71750
-0.50739,0.72253
-0.54234,0.72253
-0.55578,0.71876
-0.56317,0.71499
-0.57527,0.70494
-0.58266,0.69615
-0.58804,0.68736
-0.59341,0.67354
-0.60013,0.64717
-0.60013,0.64717
+0.10651,0.86222
+0.10651,0.86097
+0.10651,0.85721
+0.10651,0.85093
+0.10651,0.83462
+0.10651,0.81078
+0.10651,0.79949
+0.10651,0.78945
+0.10651,0.77188
+0.10651,0.75306
+0.10651,0.73800
+0.10651,0.72796
+0.10651,0.71542
+0.10651,0.69534
+0.10794,0.67903
+0.10938,0.66648
+0.10938,0.65268
+0.11081,0.61127
+0.11224,0.59747
+0.11367,0.58241
+0.11511,0.55731
+0.11511,0.54351
+0.11654,0.53222
+0.11797,0.50461
+0.11940,0.48328
+0.12083,0.46948
+0.12227,0.45567
+0.12513,0.43434
+0.12799,0.40674
+0.12943,0.39294
+0.13229,0.37035
+0.13515,0.35027
+0.13659,0.33647
+0.13945,0.31514
+0.14231,0.28502
+0.14375,0.27373
+0.14518,0.26118
+0.14661,0.25240
+0.14661,0.24738
+0.14661,0.24487
+0.14661,0.24362
+0.14661,0.24236
+0.14661,0.23860
+0.14231,0.26746
+0.14088,0.27750
+0.13515,0.31890
+0.13515,0.33271
+0.13515,0.34651
+0.13802,0.35655
+0.13945,0.36408
+0.14088,0.37537
+0.14375,0.38415
+0.14661,0.39294
+0.14948,0.40423
+0.15234,0.41301
+0.15520,0.42054
+0.15807,0.43309
+0.16093,0.44187
+0.16380,0.44940
+0.16666,0.46069
+0.17096,0.46948
+0.17382,0.47826
+0.17669,0.48704
+0.18098,0.49457
+0.18528,0.50085
+0.18814,0.50461
+0.18957,0.50712
+0.19387,0.51089
+0.19960,0.51339
+0.20819,0.51339
+0.21678,0.51089
+0.22394,0.50712
+0.23254,0.50210
+0.24256,0.49583
+0.25115,0.48955
+0.26261,0.47952
+0.27407,0.46697
+0.27980,0.45944
+0.28696,0.44940
+0.29412,0.43936
+0.30128,0.42807
+0.30844,0.41427
+0.31273,0.40423
+0.31560,0.39419
+0.31846,0.38415
+0.31990,0.37913
+0.32133,0.37160
+0.32419,0.36031
+0.32849,0.35027
+0.32992,0.34149
+0.33278,0.33647
+0.33708,0.32894
+0.33851,0.32518
+0.34138,0.32141
+0.34711,0.31514
+0.35283,0.31137
+0.35856,0.30761
+0.36286,0.30510
+0.37002,0.30259
+0.37718,0.30259
+0.38720,0.30259
+0.39723,0.30259
+0.40582,0.30385
+0.41728,0.30761
+0.42730,0.31012
+0.44306,0.31765
+0.45165,0.32267
+0.46167,0.32894
+0.47170,0.33647
+0.48172,0.34274
+0.49318,0.35027
+0.50320,0.36031
+0.51896,0.37913
+0.52469,0.39043
+0.53041,0.40423
+0.53471,0.41301
+0.53901,0.42932
+0.53901,0.43811
+0.53901,0.44940
+0.53901,0.46195
+0.53757,0.47199
+0.53614,0.48203
+0.53471,0.48955
+0.53185,0.49457
+0.53041,0.49708
+0.53041,0.49834
+0.52755,0.50085
+0.52755,0.50085
+0.52325,0.50085
+0.51896,0.50085
+0.51180,0.50085
+0.50893,0.49834
+0.50177,0.49583
+0.49461,0.49081
+0.48745,0.48579
+0.47743,0.47701
+0.47027,0.46822
+0.46597,0.46069
+0.46167,0.45442
+0.45738,0.44689
+0.45308,0.44187
+0.45022,0.43434
+0.44449,0.41929
+0.44306,0.40172
+0.44306,0.38792
+0.44449,0.37913
+0.44592,0.37160
+0.44878,0.36282
+0.45022,0.35529
+0.45308,0.34902
+0.46597,0.33647
+0.47743,0.32894
+0.49032,0.32016
+0.50034,0.31514
+0.51896,0.30761
+0.52469,0.30510
+0.53041,0.30385
+0.53471,0.30385
+0.54044,0.30385
+0.55046,0.30887
+0.56335,0.32016
+0.57195,0.33145
+0.58054,0.34274
+0.58483,0.35153
+0.59486,0.37035
+0.60202,0.38541
+0.61061,0.40046
+0.61920,0.41427
+0.62636,0.42807
+0.63353,0.44062
+0.63782,0.44940
+0.64212,0.45944
+0.64641,0.47073
+0.64785,0.47952
+0.64928,0.48579
+0.64928,0.49081
+0.64928,0.49332
+0.64928,0.49332
+0.64928,0.49457
+0.64928,0.49583
+0.64498,0.47450
+0.64355,0.46822
+0.64069,0.45317
+0.63496,0.42807
+0.63209,0.40674
+0.63209,0.39670
+0.63066,0.38917
+0.63066,0.37662
+0.63066,0.36157
+0.63353,0.35153
+0.63496,0.34274
+0.63925,0.33647
+0.64498,0.32894
+0.65214,0.32267
+0.65930,0.31765
+0.67076,0.30887
+0.67792,0.30510
+0.68222,0.30259
+0.68651,0.30259
+0.69367,0.30259
+0.70227,0.30259
+0.71229,0.30385
+0.72518,0.30887
+0.73520,0.31514
+0.74953,0.32518
+0.76385,0.34149
+0.77387,0.35529
+0.80108,0.39545
+0.80824,0.41050
+0.81397,0.42180
+0.81683,0.42932
+0.81970,0.43685
+0.82399,0.44564
+0.82686,0.45066
+0.82972,0.45944
+0.83116,0.46571
+0.83402,0.47952
+0.83402,0.48453
+0.83402,0.48955
+0.83402,0.49332
+0.83402,0.49834
+0.83402,0.49959
+0.83402,0.50210
+0.83402,0.50336
+0.83402,0.50461
+0.83402,0.50587
+0.83259,0.50461
+0.83259,0.43936
+0.83259,0.38917
+0.83259,0.34902
+0.83259,0.34274
+0.83259,0.32016
+0.83259,0.30134
+0.83116,0.29004
+0.82829,0.27122
+0.82686,0.25491
+0.82543,0.24111
+0.82256,0.22730
+0.81970,0.21099
+0.81397,0.20095
+0.80824,0.18841
+0.80108,0.17711
+0.79249,0.16707
+0.78246,0.15704
+0.77530,0.15076
+0.76385,0.14323
+0.74523,0.13445
+0.72804,0.12692
+0.70227,0.11437
+0.67935,0.10308
+0.65787,0.09304
+0.64212,0.08677
+0.63209,0.08300
+0.60918,0.07924
+0.58627,0.07422
+0.57481,0.07422
+0.55762,0.07422
+0.54044,0.07422
+0.52469,0.07297
+0.50893,0.07297
+0.49748,0.07297
+0.45308,0.07297
+0.43446,0.07422
+0.42014,0.07548
+0.39580,0.07798
+0.37575,0.07924
+0.36429,0.07924
+0.35856,0.08049
+0.34567,0.08175
+0.33422,0.08300
+0.30844,0.08677
+0.29698,0.08928
+0.28982,0.09053
+0.28123,0.09179
+0.25832,0.09555
+0.24543,0.09806
+0.23397,0.09932
+0.22538,0.10183
+0.21822,0.10308
+0.20962,0.10684
+0.19817,0.11312
+0.18957,0.11814
+0.18241,0.12316
+0.17525,0.12692
+0.16236,0.13570
+0.15520,0.14072
+0.14948,0.14825
+0.14375,0.15704
+0.14088,0.16331
+0.13659,0.17209
+0.13372,0.17711
+0.12656,0.19092
+0.12513,0.19719
+0.12513,0.20221
+0.12513,0.20974
+0.12513,0.21601
+0.12656,0.22730
+0.13086,0.23734
+0.13372,0.24487
+0.14375,0.25240
+0.15807,0.26118
+0.17239,0.26746
+0.18241,0.26997
+0.19817,0.27248
+0.21249,0.27248
+0.22681,0.27248
+0.23827,0.27248
+0.25115,0.27248
+0.26834,0.27248
+0.29125,0.27122
+0.30844,0.27122
+0.33135,0.27373
+0.36286,0.27624
+0.38577,0.27875
+0.41012,0.28001
+0.43733,0.28377
+0.45595,0.28502
+0.48602,0.28628
+0.51323,0.28628
+0.53185,0.28628
+0.57624,0.28377
+0.59343,0.28251
+0.62207,0.27750
+0.65071,0.27499
+0.66646,0.27373
+0.68651,0.27248
+0.70513,0.27248
+0.71802,0.27122
+0.72804,0.27122
+0.73520,0.27122
+0.74380,0.26997
+0.75382,0.26997
+0.76814,0.26997
+0.78246,0.26997
+0.79392,0.26997
+0.79965,0.26997
+0.80538,0.26997
+0.81254,0.26997
+0.81970,0.26997
+0.82972,0.26997
+0.84261,0.26997
+0.85407,0.26997
+0.86553,0.26997
+0.86982,0.26997
+0.87555,0.26997
+0.87698,0.26997
+0.87841,0.26997
+0.88128,0.26997
+0.88987,0.26997
+0.90276,0.26997
+0.90706,0.26997
+0.90849,0.26997
+0.90992,0.26997
+0.91135,0.26997
+0.91135,0.26997
diff --git a/python/ur_simple_control/__init__.py b/python/ur_simple_control/__init__.py
index 4cbd4c8..5cb8d28 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"
]
diff --git a/python/ur_simple_control/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/__pycache__/__init__.cpython-312.pyc
index 9b2834644867a509017a904089c126c95aadacf2..020d0f164aa0e0b0d8f6cab89bf1677599bec9c6 100644
GIT binary patch
delta 28
jcmZo?ZfE8_&CAQhz`($8I@&sY-bP+;M#fu{BN_DoWTyvr
delta 28
jcmZo?ZfE8_&CAQhz`(%ZX7VR>{zhJJM#f*0BN_DoWf}*o
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index a1ee685af1c3b75aa6819392fdc500bbc70a3e9a..96a852891d509709b84808fbed70a2d7f0248b4a 100644
GIT binary patch
delta 619
zcmezRf$8H1Cf?J$yj%<n3=A?c*6Cauc?+2tg(e5GC^HIA7G#xY6q#(us>&z|Vl#`S
zKxD+hJP8O-Sh9ozq#cA)q*A2Q8B$~>?`KjKlr7-_iGgs6T#Eb}{>hEZih>Fyd>|1J
zPEkxzf`~FIgG^*p0of&}iljm<MIE9-Py<O+6RuAStWO)Hk5LEA)&;Qz^^n}8pJD(t
z!w^Z-D8(2e%4h=CXA0J5hU8}R6bpz7K}#gNtx~Ml*noXvizI57Vh<5jbU+eyOmSMn
zznTT)R|W<~h8l)o22JP9%h`7`F@|hT;>zXbPWiyhz$)>Dfsr+2@-0z&gG@L-oACh)
z*L4>0i!9<7B=ojaTsIHBXdZe)F#AGjC0tSUW>2wRTPAj!$qvC5lLbN^Fp5q781j`-
ze6n<?98-$;WPdQVSv~AMBcsG*@d!gk$;qA(i<w?BO@0?~WAo|Ao6>BP85kIfCT(6-
zI*)~Y5(5K+pC<R@1C5fKGpa1u87FOC*x1O*IBBzTJ1d9i1dviN!NkD8@RAwK-E7-`
gid6_KYRtgEpvhPSW>4BIIdwfh<Alv8m#h&60R0t^$N&HU
delta 627
zcmezPf$8f9Cf?J$yj%<n3=FFy%+q-{@)j~rHeyv_6q+o^D$gi9*^pJ0Q3T8u1+&G#
zY;h1<SfYdjq!ENuBvYi)8B(Mt?`KjKlqul>iGgs6Y>M0({>hEZih}Ybd>|1JPEklv
zgorXKfz49}nJ1`%q(U`C4WdF&9Z6IJu1^!JPYa}vQ5($G0kH*jk=&%0q7OC007=v^
z#RwwGXbje80@i1W<Yu!JbBGE-3naTOQ>@llgMDIyBx;*t2N6}YM-p{Naa_Z{ng!%n
z1_nlk8irs7O{dMv*>^K-PUgzx=1%><%)lz~g@KVZbn<OcdxI=EKZo%F3)giP@rx|t
z7bNtyR9rU?y=WeKLooY7X%$>i&1NsLURx$Ho5_yB7L#*B<R<Hf9AXro{3+xsqr_yH
zP&uX)iOB(AYO_Yzdqzgd$r2HUj8c=mA{H|hDNlYMaf9h4)8-?Qccj@SF)%O`P29Y!
zbRG-)L<R;1KTYn*dm1G-r&L+6Gfvz*x3Q6xapGqAc2*9levncy!NkD8@RAwE)#RJ(
m(4)LLv40V(5Lnunfq_Aju?WnbxLI`SdVa?K%}1B45e5L%vXd?V
diff --git a/python/ur_simple_control/basics/__init__.py b/python/ur_simple_control/basics/__init__.py
index e69de29..71b8f3e 100644
--- a/python/ur_simple_control/basics/__init__.py
+++ b/python/ur_simple_control/basics/__init__.py
@@ -0,0 +1 @@
+from .basics import *
diff --git a/python/ur_simple_control/basics/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/basics/__pycache__/__init__.cpython-312.pyc
index bfdbb02db5158c96d88c7565cf618180c158a478..e59be1cb003dba22092cd0ab31c6aa7026ae1251 100644
GIT binary patch
delta 126
zcmcb}_?9u?G%qg~0|Ns?VU%^c7y|>tV-N?1nV^i%d<+Z>(-~42QW$d>)-o|NR5EEY
zzhq=!V9;c|#i-?{$#{z`DX};+xp?J7^9t2l95%W6DWy57c10Wv3=AN3#URE9W=2NF
My9}a5Yzzzx0N@lDxBvhE
delta 98
zcmaFMc#+ZkG%qg~0|Ntto5`P45d9cLpySUh3=9m@87dhx8U0o=6frR{Obn<{`o&?B
ho1apelWJGQ%D}+D$iTo*3}Sp_W@Kb6VrF1q0040X5gGsh
diff --git a/python/ur_simple_control/clik/__init__.py b/python/ur_simple_control/clik/__init__.py
index e69de29..89ff317 100644
--- a/python/ur_simple_control/clik/__init__.py
+++ b/python/ur_simple_control/clik/__init__.py
@@ -0,0 +1 @@
+from .clik import *
diff --git a/python/ur_simple_control/clik/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/clik/__pycache__/__init__.cpython-312.pyc
index da51c5e2ade3792316b9ed1771c9d1b7da7ccf70..2a81ddc6cd46f2c3255161f2c7285382187bf2c9 100644
GIT binary patch
delta 124
zcmX@l_>$4@G%qg~0|NuY<1p)V5fBaHz%Ub(@tKc-fnhpB3PTEG4#Qd|MutizP3D)3
z3=9mKjJFuI{4^PFu_WhYX0M!RUZ#4B!zMRBr8Fniu84zyfdQnd7{vI%%*e=imqECQ
Ije&sy0LhvdL;wH)
delta 98
zcmaFKc%ISxG%qg~0|Ntto5`P45d9cLpySUh3=9m@87dhx8U0o=6frR{O!O;L`o&?B
ho1apelWJGQ%D}+D$iTo*3}Sp_W@Kb6VrF1q003?)5fA_X
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index ff4d882..830838a 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -146,8 +146,9 @@ def controlLoopClik(robot : RobotManager, clik_controller, i, past_data):
breakFlag = False
log_item = {}
q = robot.getQ()
+ T_w_e = robot.getT_w_e()
# first check whether we're at the goal
- SEerror = robot.data.oMi[robot.JOINT_ID].actInv(robot.Mgoal)
+ SEerror = T_w_e.actInv(robot.Mgoal)
err_vector = pin.log6(SEerror).vector
if np.linalg.norm(err_vector) < robot.args.goal_error:
# print("Convergence achieved, reached destionation!")
@@ -221,7 +222,7 @@ def moveUntilContact(args, robot, speed):
time.sleep(0.01)
print("Collision detected!!")
-def moveL(args, robot, goal_point):
+def moveL(args, robot : RobotManager, goal_point):
"""
moveL
-----
@@ -240,19 +241,20 @@ def moveL(args, robot, goal_point):
}
loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
loop_manager.run()
- # TODO: remove, this isn't doing anything
- time.sleep(0.01)
- print("MoveL done: convergence achieved, reached destionation!")
# TODO: implement
-# the idea is to make a path from current to goal position
-# which is just a straight line between them.
-# the question is what to do with orientations.
-# i suppose it makes sense to have one function that enforces/assumes
-# that the start and end positions have the same orientation.
-# then another version goes in a line and linearly updates the orientation
-# as it goes
def moveLFollowingLine(args, robot, goal_point):
+ """
+ moveLFollowingLine
+ ------------------
+ make a path from current to goal position, i.e.
+ just a straight line between them.
+ the question is what to do with orientations.
+ i suppose it makes sense to have one function that enforces/assumes
+ that the start and end positions have the same orientation.
+ then another version goes in a line and linearly updates the orientation
+ as it goes
+ """
pass
# TODO: implement
@@ -340,9 +342,6 @@ def compliantMoveL(args, robot, goal_point):
}
loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
loop_manager.run()
- # TODO: remove, this isn't doing anything
- time.sleep(0.01)
- print("compliantMoveL done: convergence achieved, reached destionation!")
def clikCartesianPathIntoJointPath(args, robot, path, \
@@ -389,6 +388,7 @@ def clikCartesianPathIntoJointPath(args, robot, path, \
# loop logs is a dict, dict keys list preserves input order
new_q = sim_robot.q.copy()
robot.updateViz(sim_robot.q, sim_robot.T_w_e)
+ time.sleep(0.05)
qs.append(new_q[:6])
# plot this on the real robot
diff --git a/python/ur_simple_control/dmp/__init__.py b/python/ur_simple_control/dmp/__init__.py
index e69de29..79beb15 100644
--- a/python/ur_simple_control/dmp/__init__.py
+++ b/python/ur_simple_control/dmp/__init__.py
@@ -0,0 +1 @@
+from .dmp import *
diff --git a/python/ur_simple_control/dmp/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/dmp/__pycache__/__init__.cpython-312.pyc
index 1391575a09999c264117e9287f69a1b2d7b7006d..76ca428c42d8c7da269804cca80bbe7ef51db740 100644
GIT binary patch
delta 123
zcmX@d_?*%AG%qg~0|Ns?S(J6UFara_V-N?1nV^i%d<+Z>(-~42QW$d>)-o|NR5EEY
zzhq=!V9;c|#i-?{$#{!7CAVPZMDtSBTO2mI`6;D2sdhyi3=9k)Ma3Y-2WCb_#=8tc
JMQjWV3;=q?7jXap
delta 98
zcmaFPc#hHhG%qg~0|Ntto5`P45d9cLpySUh3=9m@87dhx8U0o=6frR{O!O^P`o&?B
ho1apelWJGQ%D}+D$iTo*3}Sp_W@Kb6VrF1q003;h5eonS
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 42cd882..d09e663 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -78,13 +78,13 @@ def getMinimalArgParser():
help="robot's ip address (only needed if running on the real robot)", \
default="192.168.1.102")
parser.add_argument('--pinocchio-only', action=argparse.BooleanOptionalAction, \
- help="whether you want to just integrate with pinocchio", default=False)
+ help="whether you want to just integrate with pinocchio", default=True)
parser.add_argument('--fast-simulation', action=argparse.BooleanOptionalAction, \
help="do you want simulation to run over 500Hz? (real-time viz relies on 500Hz)", default=False)
parser.add_argument('--visualize-manipulator', action=argparse.BooleanOptionalAction, \
- help="whether you want to visualize the manipulator and workspace with meshcat", default=False)
+ help="whether you want to visualize the manipulator and workspace with meshcat", default=True)
parser.add_argument('--real-time-plotting', action=argparse.BooleanOptionalAction, \
- help="whether you want to have some real-time matplotlib graphs (parts of log_dict you select)", default=False)
+ help="whether you want to have some real-time matplotlib graphs (parts of log_dict you select)", default=True)
parser.add_argument('--gripper', type=str, \
help="gripper you're using (no gripper is the default)", \
default="none", choices=['none', 'robotiq', 'onrobot'])
@@ -457,7 +457,7 @@ class RobotManager:
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(5)
+ time.sleep(2)
if args.debug_prints:
print("ROBOT_MANAGER: manipulator_visualizer_process started")
self.manipulator_visualizer_queue.put(np.zeros(self.model.nq))
diff --git a/python/ur_simple_control/optimal_control/__init__.py b/python/ur_simple_control/optimal_control/__init__.py
new file mode 100644
index 0000000..255cf7b
--- /dev/null
+++ b/python/ur_simple_control/optimal_control/__init__.py
@@ -0,0 +1,4 @@
+from .create_pinocchio_casadi_ocp import *
+from .crocoddyl_mpc import *
+from .crocoddyl_optimal_control import *
+from .get_ocp_args import *
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
new file mode 100644
index 0000000..d0e2717
--- /dev/null
+++ b/python/ur_simple_control/optimal_control/create_pinocchio_casadi_ocp.py
@@ -0,0 +1,149 @@
+import casadi
+import pinocchio as pin
+from pinocchio import casadi as cpin
+import numpy as np
+from ur_simple_control.managers import RobotManager, ControlLoopManager
+import pickle
+from importlib.resources import files
+
+"""
+CREDIT: jrnh2023 pinocchio.casadi tutorial
+Implement and solve the following nonlinear program:
+decide q \in R^NQ
+minimizing sum_t || q - robot.q0 ||**2
+so that
+ h(q) = target
+ forall obstacles o, (e_p - e_c)' e_A (e_p-e_c) >= 1
+ TODO: here also define the table/floor as a plane, which would be
+ normal_plane_vector (0,0,1) \cdot T_w_e.translation > 0
+with h(q) the forward geometry (position of end effector) to be at target position,
+e_A,e_c the ellipse matrix and center in the attached joint frame e_, and e_p = oMe^-1 o_p
+the position of the obstacle point p in frame e_.
+
+The following tools are used:
+- the ur10 model (loaded by example-robot-data)
+- pinocchio.casadi for writing the problem and computing its derivatives
+- the IpOpt solver wrapped in casadi
+- the meshcat viewer
+
+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
+ 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()
+
+ for ellipse in ellipses:
+ ellipse.id = 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: 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 = []
+
+
+ cmodel = cpin.Model(robot.model)
+ cdata = cmodel.createData()
+
+ #######################################################################
+ # 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],
+# )
+ 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))]
+ )
+ #######################################################################
+ # dynamics level #
+ #######################################################################
+
+ # variables on a single time-step
+ nx = robot.model.nq + robot.model.nv
+ ndx = 2 * robot.model.nv
+ cx = casadi.SX.sym("x", nx, 1)
+ cdx = casadi.SX.sym("dx", robot.model.nv * 2, 1)
+ cq = cx[:robot.model.nq]
+ cv = cx[robot.model.nq:]
+ 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
+ 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,
+ )
+ ],
+ )
+ opti = casadi.Opti()
+ # 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
+ totalcost = 0
+ for t in range(T):
+ # running
+ totalcost += 1e-3 * DT * casadi.sumsqr(var_xs[t][nq:])
+ totalcost += 1e-4 * DT * casadi.sumsqr(var_as[t])
+ # terminal cost
+ totalcost += 1e4 * casadi.sumsqr(error_tool(var_xs[T]))
+
+
+ # collision avoidance (hard) 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):
+ opti.subject_to(cnext(var_xs[t], var_as[t]) == var_xs[t + 1])
+ for ellipse in ellipses:
+ for obstacle in obstacles:
+ for x in var_xs:
+ # obstacle position in ellipsoid (joint) frame
+ #e_pos = e.e_pos(var_q, o.pos)
+ e_pos = ellipse.e_pos(x[:nq], obstacle.pos)
+ opti.subject_to((e_pos - ellipse.center).T @ e.A @ (e_pos - e.center) >= 1)
+
+ # now that the ocp has been transcribed as nlp,
+ # solve it
+ try:
+ sol = opti.solve_limited()
+ #sol_q = opti.value(var_q)
+ sol_xs = [opti.value(var_x) for var_x in var_xs]
+ sol_as = [opti.value(var_a) for var_a in var_as]
+ except:
+ print("ERROR in convergence, plotting debug info.")
+ #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]
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
index eb07d99..0b9a45d 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -9,21 +9,6 @@ from ur_simple_control.visualize.visualize import plotFromDict
import example_robot_data
import time
-
-def get_OCP_args(parser : argparse.ArgumentParser):
- parser.add_argument("--ocp-dt", type=float, default=1e-2, \
- help="time length between state-control-pair decision variables")
- parser.add_argument("--n-knots", type=int, default=100, \
- help="number of state-control-pair decision variables")
- parser.add_argument("--stop-at-final", type=int, default=1, \
- help="the trajectory generated by the OCP will be followed. it might not have finally velocity 0. \
- if this argument is set to true, the final velocity will be set to 0 (there will be overshoot).")
- parser.add_argument("--solver", type=str, default="boxfddp", \
- help="optimal control problem solver you want", choices=["boxfddp", "csqp"])
- parser.add_argument("--max-solver-iter", type=int, default=200, \
- help="number of iterations the ocp solver takes. ~100-500 for just ocp (it can converge before ofc), ~10 for mpc")
- return parser
-
def createCrocoIKOCP(args, robot : RobotManager, x0, goal : pin.SE3):
# create torque bounds which correspond to percentage
# of maximum allowed acceleration
diff --git a/python/ur_simple_control/optimal_control/get_ocp_args.py b/python/ur_simple_control/optimal_control/get_ocp_args.py
new file mode 100644
index 0000000..97d87a1
--- /dev/null
+++ b/python/ur_simple_control/optimal_control/get_ocp_args.py
@@ -0,0 +1,15 @@
+import argparse
+
+def get_OCP_args(parser : argparse.ArgumentParser):
+ parser.add_argument("--ocp-dt", type=float, default=1e-2, \
+ help="time length between state-control-pair decision variables")
+ parser.add_argument("--n-knots", type=int, default=100, \
+ help="number of state-control-pair decision variables")
+ parser.add_argument("--stop-at-final", type=int, default=1, \
+ help="the trajectory generated by the OCP will be followed. it might not have finally velocity 0. \
+ if this argument is set to true, the final velocity will be set to 0 (there will be overshoot).")
+ parser.add_argument("--solver", type=str, default="boxfddp", \
+ help="optimal control problem solver you want", choices=["boxfddp", "csqp"])
+ parser.add_argument("--max-solver-iter", type=int, default=200, \
+ help="number of iterations the ocp solver takes. ~100-500 for just ocp (it can converge before ofc), ~10 for mpc")
+ return parser
diff --git a/python/ur_simple_control/util/__init__.py b/python/ur_simple_control/util/__init__.py
index e69de29..fabba21 100644
--- a/python/ur_simple_control/util/__init__.py
+++ b/python/ur_simple_control/util/__init__.py
@@ -0,0 +1,8 @@
+#from .calib_board_hacks import *
+#from .draw_path import *
+#from .encapsulating_ellipses import *
+#from .freedrive import *
+#from .ft_calibration import *
+#from .get_model import *
+#from .logging_utils import *
+#from .map2DPathTo3DPlane import *
diff --git a/python/ur_simple_control/util/__pycache__/__init__.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/__init__.cpython-312.pyc
index 3fbe9907280c84609fe3b1cea8305bb8ce0c7421..52194c1d073869e8b7aa28fc186e22a92633ed64 100644
GIT binary patch
delta 26
icmX@lc%G5>G%qg~0|NuY?`Z4v?-O|!Fy5NDP7wfXtqBDH
delta 26
icmX@lc%G5>G%qg~0|Ntto5`P4hKalj7=KM%rw9OHum}+V
diff --git a/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-312.pyc
index 4ac2f3a069d20ebbb0e8dd7eac2be20e189b1bbe..54d7c441ab3d60c2b2f3e0a916348d21a5b09f9b 100644
GIT binary patch
delta 74
zcmZq8Y|i98&CAQhz`(%Z7j2zhypdOeg;8>|8p~}qM)l21+*4T@-8Rn<2;gB%*!)sv
fuOQ>H&CB)LS$TXI7#Kb<Gcq#XW)PojW8w(_<tY>O
delta 76
zcmZq9Y|Z37&CAQhz`(%J5NMu$X(O)$3!}_tHJ00Kj9QzSxTmr*dTyQ}5WvHjy!oZf
hUO~o{o0sdgv-0^dFfe>zW@Kdi%)!7YG1<z*696pR6srIL
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc
index ff30b90edef8b10d19dfe9251aa3735474ea1fab..b58f1882fb35dc03e56f929f8a076affb090553c 100644
GIT binary patch
delta 1199
zcmdn$u+o|DG%qg~0|Ns?Z=7{HpUOr)3q~dhj?Es7fo$~>SqdN-RP4eKtI5DnE14ot
zBblXyDhJ|~F%&cQ2-QetsepK>xK^r0Dn+nLh=Bpbs*#Krg30%2voO?%quN&wk))x8
zY&Bw8Y7kqH$rK?Q1_p+jdQKeD!eD709I_%{S$-U{qF`Ae9I|3ySy6~AM&PE1gQQFN
zVQ$URnEZfEWO5D%C%<%!SdDm%M2&PBOE80`#O4-`xs3d`STYjxQl9@=`SFr(Qh@zr
zMJ{<p(aDZnlP%Lz^K(;6iYon+vQm>vif^&xCFZ8y;>=Ag&InFU%t^h)pOv4PR}zqu
zn4FrMng>$hl30?cDYN-CR}P~PNc%0u;#({!i6x1}G7Jn1lM{I)CztayaN9F5Fo0sd
zSdw`&6R!aiBhTcYateMAcz7p>c6xO<-jJ2+aOw5?z|A12`+<i+P~p0O{uKfJ4wf6-
zf)hffM^22q#4UG2Kx8`KM7|D3Y}yPy2y8ay_h3}9U|?V<vIG%UAi|b`fkBh;7Hd&{
zQhrG>BLf42LXq9%`2rHGZVU_znUnVj*s(Z(oNB-*wpm2*3uC4TNSg$RkOUD@AVL~M
zfP7S>2r`eYNEsxc3L>!BSIh>oMgath>=+mrZt<t5mc(bI7Uk!~=jNxR<`j8=bWSc5
zUdiaNSz6>aD;p<BfMfC>Ne`z05Yr!|jU_3uIJF35SP?tOTKOVSI2M5%c#G8+Y>*sC
z9j`ky;viz8lY6AP*+7}7NO-cWbSzhqEXYWCkT0i6A7u2K93i763wAd=h>EO1N<ayz
zSQw-(kw;ho;&x$(yAulvit;Nma}!H4^Ye<z7#J8jHeZzKWMXuf94#*;4slanX>LJf
z5y<9S%*B<(Mf{U{<kjqAK}I`+2(a@(;SLD^kSQw}ia@~+v5UhdH$SB`C)KW~j)8#z
m6j{Y~lRwC7OFJ_%ntk$OVq)}T{3Im8X!cphg^|$;tP=o%7x2yi
delta 803
zcmZ4Kyw!p4G%qg~0|<y3rSDYS$Y;UGWWm1KgE5fJk*!87O93Q>ic<t^7#J98IC01d
zf@OI?vXkpLBw6{vtT`N#wn88ls#zdjiU?STC=OXskZcM6<aH7<lf5{(`6X+_YQ$?K
zY9!NGf*CZ$Hm7jTWt=R)Ezc-4*?@bpcvgO9UP(YsVsdJ3YF<h4Ef$x=l0;3Z&40La
z7&%iCOA?Ev7#J8PH}Z;3p1{jGc^Ypmw*><Og9-x!L-A^+&2oGOOpI)s_X>D0IvO)D
zFcg`92vZPY#=yX!$#{#kC_gE`q?nO`fkC0joPmMimQZ?XiF;9IK|yL!PHJ&+Xpm`Y
zkv#(gL&Rh^Av+dJkaY%(Vw>j)ePQ$$0+}QRBE&(21c;CX5g=C;$$+e2E0O~VfI_|q
zixZ03KsGCYK#}?6ha#e^jtmS8iIabetYoy@yj1iyD;qmVKilLsX%7!i5Yq$XN|vO=
z;?!F#8Hsr*>3>#!yyTk{V6Q1(1oC_l$W^ykee+XNb8fNZCFZ8y;&o5W&rK~Usssxb
z2~QS~>1GoE874S+p-imIE&g<<0{^6})Z~)lTb#M6#TmiLi8-m0Ib;tqx=%hOt0fM0
zKRonqv4Aw)Vl1A_BPS^j4m;tD)S~>n_{4&OqWp@?+{BX1{Jf%41_p-m%`S4COpKP3
z&nZZWL)@2Fnp;p=1aibJ=HklYBHqdW71Zn^K#l^X5r|hn0bc|P5wIyM8HzyR4~ZuZ
yo80`A(wtPgqFM$922cbR+fGha)Rr=0H2ajn$i(Q!_(@2F(d@I13nQZySR(-Ym$w)I
diff --git a/python/ur_simple_control/util/calib_board_hacks.py b/python/ur_simple_control/util/calib_board_hacks.py
index ef42370..f753a18 100644
--- a/python/ur_simple_control/util/calib_board_hacks.py
+++ b/python/ur_simple_control/util/calib_board_hacks.py
@@ -1,6 +1,3 @@
-# TODO: make prints prettier (remove \ in code, and also
-# make it look good in the terminal)
-
import pinocchio as pin
import numpy as np
import time
diff --git a/python/ur_simple_control/util/encapsulating_ellipses.py b/python/ur_simple_control/util/encapsulating_ellipses.py
new file mode 100644
index 0000000..3e20424
--- /dev/null
+++ b/python/ur_simple_control/util/encapsulating_ellipses.py
@@ -0,0 +1,185 @@
+"""
+CREDIT: jnrh2023 pinocchio tutorial
+
+Optimization of the shape of an ellipse so that it contains a set of 3d points.
+Used to construct ellipses around manipulator links to get convex self-collision checking.
+NOTE: probably cylinders are better but let's run with this.
+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: (pk-c)@A@pk-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.
+"""
+
+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.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):
+ # 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))
+ y = radii[1] * np.outer(np.sin(u), np.sin(v))
+ z = radii[2] * np.outer(np.ones_like(u), np.cos(v))
+ for i in range(len(x)):
+ for j in range(len(x)):
+ [x[i, j], y[i, j], z[i, j]] = (
+ 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)
+
+
+# plotting the vertices in the ellipse in matplotlib for verification
+def plotVertices(ax, vertices, nsubsample):
+ """
+ plot a 3xN array of vertices in 3d. If nsubsample is not 0, plot the main once in
+ large red, the others in small green.
+ """
+ vert = vertices
+ NS = nsubsample
+
+ # Plot the vertices
+ ax.plot3D(vert[::, 0], vert[::, 1], vert[::, 2], "g.", markersize=1)
+ ax.plot3D(vert[::NS, 0], vert[::NS, 1], vert[::NS, 2], "r*")
+
+ # Change the scalling for a regular one centered on the vertices.
+ m, M = np.min(vert, 0), np.max(vert, 0)
+ plot_center = (m + M) / 2
+ plot_length = max(M - m) / 2
+ ax.axes.set_xlim3d(
+ left=plot_center[0] - plot_length, right=plot_center[0] + plot_length
+ )
+ ax.axes.set_ylim3d(
+ bottom=plot_center[1] - plot_length, top=plot_center[1] + plot_length
+ )
+ ax.axes.set_zlim3d(
+ 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))
+
+ellipses = []
+for geom in 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])
+# 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)])
+
+ """
+ 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
+
+ #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)
diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py
index 8842981..c53ade8 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -74,6 +74,23 @@ def get_model():
visual_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.VISUAL, None, mesh_dir_absolute)
collision_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.COLLISION, None, mesh_dir_absolute)
+ # for whatever reason the hand-e files don't have/
+ # meshcat can't read scaling information.
+ # so we scale manually,
+ # and the stupid gripper is in milimeters
+ for geom in visual_model.geometryObjects:
+ if "hand" in geom.name:
+ s = geom.meshScale
+ # this looks exactly correct lmao
+ s *= 0.001
+ geom.meshScale = s
+ for geom in collision_model.geometryObjects:
+ if "hand" in geom.name:
+ s = geom.meshScale
+ # this looks exactly correct lmao
+ s *= 0.001
+ geom.meshScale = s
+
# updating joint placements.
model.jointPlacements[1] = shoulder_se3
model.jointPlacements[2] = upper_arm_se3
diff --git a/python/ur_simple_control/util/grippers/__init__.py b/python/ur_simple_control/util/grippers/__init__.py
new file mode 100644
index 0000000..e69de29
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 9dd45e1af3f798ed65e27a6b9dcc08e32431e013..73420587c7f2ef3c0238de3d2df72ad882fa8306 100644
GIT binary patch
delta 1910
zcmey}!T7J4k?%AwFBby?14Bcub^1e>jeHlG7zH;!VEV|ysJ{6UyPN={)8>!j4_O3)
zSQr?lGo&!qvZgTBvXwASzOOFM%3i~s#XR{Sr-;BB=GBZKV;C418EV+GSSKfP%F41s
zL>L%SSP-(D44e!#ELohB8QG+aYB_2+To@)W#`-ZZ)N<Bvrm$vfFfbIA)-Ys&T?I1$
zLKktDuuoQy6Je=g&*GV^$S*wEjYVW~p`0*Nmhj|49a&}!VbRHqY%(g040%^-I8)fc
zW{1_VrZ9q8Y9-<ja~T-2BqnbZ6k*EZnS78_Sejjufq@}~V-4qOCWrz?h7_h+R<NxH
zIfdE5@>$Z8Hwp@Ka-=|bGLsqEgeNCxa>;Y1aDj~vt6?K#5f@gA*x(j{<>3}VcyNmZ
zIa9bn78Ns1VC=E3Vb78WIf$7fg?sWF1!+eGh|?JuR2YiN7#VPfC`?@n4<eGlE+9I9
zC;w68W#;vBn=Gw#kx_6mzw#VLuE}eZgN2#PGOKPeC+DWzV$M#jECxmCWLXt;xmE@S
zh9wLP48^Az85kNEK5#Hd%3WX)y&)pm;QNJR^97atj7A3;TzTBo8IJKuf=G2o8xJ++
z6RM0JYRt)OFefuGFt9Q(FfcPPFnpfGI{BcQETit^`)W!&B_Lt2sw}q2@*1MDOp6#{
zzN=x2X8}twFfjD!voMr!gVHiHdky>K{hXqL%xjPmYYlrAKS;kY>mtTAY%mkRNn8+=
zRQTCZ#Du|Os98Y-B*x7FifK@4&k_gm1vyi=5Q(dX11id$!h<O)&YQxM&X~ei#mc~d
zkcGIKm9vI3ix-l1MPMnAnKO$YYzhAw0VL<KWJyk*s436RRm)w&4oOs#c}%%kYB-^s
z1=>8!tXZ-kt&^+Fx#ddaVZoFlgfIu1e<eZrx4xFAL=mQ_2A+OOlwe|@^jXw`WRNhz
zpc<YO5wLto4MUa+Ob3E4;zrUZilmP{O9ZNFvIaj7quS(&I_iwt&;mdNR4zcoCKu?)
z3RE)G@We1NFw_$3ZM~lR$_yoXP#X}r2ZswMZ`2ZHX3s*h7MD{%xx`G|?-ol&VqS_S
z%kx<?LExl)Q3nG91A+)C%4T3-&}2b~6m?8C&<$YA11sCyrpv>~C^7l1lDJ}#3`i(|
zfq~%`XKre7hI4*Seo+x9=@%7(WQryq)>BU`2Fddjr<TMQ6y+DB7L`;M`GRC?K!gv7
z@Bk5hAOaNQMeZP035X~K5oI8v97M!|9L8G&3Y(%@kU$-XsGsbuFUfI>B|SAix2S1y
zfxZN10Vr?tGB7Yqo}_PLH=BWh;VMYPnhBKKZwSa-7tpyPpwq!}gIjPy$n?mGk(apT
z?#Rl|kC+*8NzMGStVM@Quipn&20`5~Y?Cjj$ZRe#_{=DIRNYC<lbzuNpQR@|!%22W
z8*et|)2xi%Y?~X5Y`Gb;C-1Y3W9;25YbVOcxM;GmeJa}&ko%@io@ejRxMcD>`w-R*
z3=9lLo828&Gs{<i<U~P)8HmUP5ukFZ2;|(N4U+|3q=I%bfa)^_hGGW>28IvJjEs!8
z8HDaK$lqtsyv@LUn}Pc-gW+d321c<@0>X?^pLE0+`9GP(GcpQ)a$(kC)cusi7|6(|
R`?-oKnUPVrNQ{Aj0RY~1j9mZ#
delta 2436
zcmey@%=o*5k?%AwFBby?14Ey+R(h%XM!t(ojQpD)FnwfURNZ`uT~2_}Ve?1vhb#g{
ztPBj(8B!Q)SyLEm*=m?43krzvxG=<OGBDJ#r!dv9PmWcUGOJ<FVgZ?kifcJ)I8vCa
zgcuk=tQz)sCYXGWHVZ=y6ZOn0VW0d_O`4H&@<C2vL6$YFs~I6SF*4L}W^qqW<dl`>
zg@`aPq_81mIT<(^YFH*as*6mP*Wi@ks^M~Bn7|n8$G}j_UBjKio~^;aP*hqo*-=hZ
zrii-)VFm}nj2g}?p~;H;!jqNMg(n~65@E`coXp53!-6h8`HLLL77iIk21bUwD>d9H
zoM7X_YFJYk!7Q~B*~u3Sp*jz83QKcJGB7ZtaIN894GlR)h7^`sR<OwjIfdE5@>z<L
zHwp?fbER-j-pH*W!N^d<ox%e)FRX?Qm&rU>O=g3e43>wR%mXr61Z*-d$mC+C35-3~
zHJn+hP^)<-^C(CQsKJ6*g`ud7kpWAv@sS^FllN-z3-SB8-D1f|%u9LxXXVFBzDWW0
zlXVm?DvR9WPfyLyO)V*^^iRr4O)e?E#gdnpn|g~gH?=q;I5{yVb@D&OIgC7$XDbEE
zv4GUxVl2MJT$WjNi#a(r<rZ^xYGpAG0|SGC!emBe_4*zL28Klp3=GAVObiSS3?Dcc
zB;_u!h~5y9Z1DZS#vmwrT|noGfKCU?4Q{~+A=4u#Mqc8UyCEPloo^yvhvNet-U*_e
zULB4%WaT<sdi_3dGYIN_;9(F{xGtc7ML?gRHeQ52gD*UjZ|cfyPEuaXD0#@hmB&Mz
z;kccohd9FtaYq|3G3HaEj9y}s<y6HvjMzX~6_Q0+To__qCx@yk8P%|1&6?~r>?us(
ztjS)(Uc-jSnkDR$8w_L=(iu{i7cs)3tA;I}6&ATY?y$TIvP2VPi5aPufbxF{@8pka
z;>;X19FrZ@MJ-WtEk~9h%svF2&XB^kh;a=&%vFpGH5^&OlM~fMkkb`MmMEwgnCvOb
z%~Zny%8@)N;EYnsS;Ltn36c}!N<qp}a8aHVUPMNNiHh^3@TN1S@K>=iFd$^1j_0c3
z0u>%mdnWf=aj-Em)No}9f^6XzSR;t!RF=t!n&1L}Cx!`BzSQ#6aAe7XGSK81nmo*0
z5H>##k`~r1d5}Sqf2niJlqkXiB1IUX1X_SWOy+dsEm4N4s^NvHDp7%nf%0xq3zA79
zFg1(}HM}XJVEK}o$s068rHZ(b)QKUf0|)8kUM*2ZEpV}CrYQ^aKU|Pee=;MREI&gH
zFFAe{?YXbaP+|acHLNtjaFsYYu9BI&UyIjD!mlWsfq|hYhk=1XljZrWnILe|zNil*
z2q!{{a>4S*!jqY_1K0|{$~H%6^DqiX78QedB_N`dfq`LijgB;18HiUtd9IGSWCa5Q
z!!4fT)ROpuqWpr?qLRuYUyy9$<hweu^_3uI6^N(?5j7wp7Gx=3ksgTM1R|P2L<`6q
z&g9&b_`=fE($rg=iOJazW>Gsxz5_&bf)ugj<)@_HVkyh4GP=c7Xn2dM(5R>gRGRUD
zOE?82Fu6omy?!181H(O#d_SzTBdLJv@%zHYAg3@tVrIl8HS^1|7T_{Z5LD&~5mV*~
zqnDKiUwAg()a7TAJZA2s<|WQ>QqIy#oZ*zXqm7Rk^BGY_AF<7h2DaRc&6Dk|;}|Dz
zo@Xt}$hdU!7MoPI8K6LzIa%J;pK<x*eA^J#%?u0-UYk$Yu4b;U1<8qm2s03o2_irR
zeG$n2MIePM8H&U}q9UMB<gm%jPbtkwwJX}gz`y`%P!!uSFfe>zW@Kc%%^-A_LH<62
z=4}S<+YH=y84PbTh<*|fW|aD*BgV-8$t<3cQTUS!vpJ*grwYadMn>Jw5nRm4jEury
F3jhskBi;Z2
diff --git a/python/ur_simple_control/visualize/meshcat_viewer_wrapper/__init__.py b/python/ur_simple_control/visualize/meshcat_viewer_wrapper/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/python/ur_simple_control/visualize/meshcat_viewer_wrapper/colors.py b/python/ur_simple_control/visualize/meshcat_viewer_wrapper/colors.py
new file mode 100644
index 0000000..d97cb12
--- /dev/null
+++ b/python/ur_simple_control/visualize/meshcat_viewer_wrapper/colors.py
@@ -0,0 +1,49 @@
+import meshcat
+
+
+def rgb2int(r, g, b):
+ """
+ Convert 3 integers (chars) 0 <= r, g, b < 256 into one single integer = 256**2*r+256*g+b, as expected by Meshcat.
+
+ >>> rgb2int(0, 0, 0)
+ 0
+ >>> rgb2int(0, 0, 255)
+ 255
+ >>> rgb2int(0, 255, 0) == 0x00FF00
+ True
+ >>> rgb2int(255, 0, 0) == 0xFF0000
+ True
+ >>> rgb2int(255, 255, 255) == 0xFFFFFF
+ True
+ """
+ return int((r << 16) + (g << 8) + b)
+
+
+def material(color, transparent=False):
+ mat = meshcat.geometry.MeshPhongMaterial()
+ mat.color = color
+ mat.transparent = transparent
+ return mat
+
+
+red = material(color=rgb2int(255, 0, 0), transparent=False)
+blue = material(color=rgb2int(0, 0, 255), transparent=False)
+green = material(color=rgb2int(0, 255, 0), transparent=False)
+yellow = material(color=rgb2int(255, 255, 0), transparent=False)
+magenta = material(color=rgb2int(255, 0, 255), transparent=False)
+cyan = material(color=rgb2int(0, 255, 255), transparent=False)
+white = material(color=rgb2int(250, 250, 250), transparent=False)
+black = material(color=rgb2int(5, 5, 5), transparent=False)
+grey = material(color=rgb2int(120, 120, 120), transparent=False)
+
+colormap = {
+ "red": red,
+ "blue": blue,
+ "green": green,
+ "yellow": yellow,
+ "magenta": magenta,
+ "cyan": cyan,
+ "black": black,
+ "white": white,
+ "grey": grey,
+}
diff --git a/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py b/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py
new file mode 100644
index 0000000..82ebdf4
--- /dev/null
+++ b/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py
@@ -0,0 +1,103 @@
+import random
+
+import meshcat
+import numpy as np
+import pinocchio as pin
+from pinocchio.visualize import MeshcatVisualizer as PMV
+
+from . import colors
+
+
+def materialFromColor(color):
+ if isinstance(color, meshcat.geometry.MeshPhongMaterial):
+ return color
+ elif isinstance(color, str):
+ material = colors.colormap[color]
+ elif isinstance(color, list):
+ material = meshcat.geometry.MeshPhongMaterial()
+ material.color = colors.rgb2int(*[int(c * 255) for c in color[:3]])
+ if len(color) == 3:
+ material.transparent = False
+ else:
+ material.transparent = color[3] < 1
+ material.opacity = float(color[3])
+ elif color is None:
+ material = random.sample(list(colors.colormap), 1)[0]
+ else:
+ material = colors.black
+ return material
+
+
+class MeshcatVisualizer(PMV):
+ def __init__(
+ self,
+ robot=None,
+ model=None,
+ collision_model=None,
+ visual_model=None,
+ url=None,
+ autoclean=False,
+ ):
+ if robot is not None:
+ super().__init__(robot.model, robot.collision_model, robot.visual_model)
+ elif model is not None:
+ super().__init__(model, collision_model, visual_model)
+
+ if url is not None:
+ if url == "classical":
+ url = "tcp://127.0.0.1:6000"
+ print("Wrapper tries to connect to server <%s>" % url)
+ server = meshcat.Visualizer(zmq_url=url)
+ else:
+ server = None
+
+ if robot is not None or model is not None:
+ self.initViewer(loadModel=True, viewer=server)
+ else:
+ self.viewer = server if server is not None else meshcat.Visualizer()
+
+ if autoclean:
+ self.clean()
+
+ def addSphere(self, name, radius, color):
+ material = materialFromColor(color)
+ self.viewer[name].set_object(meshcat.geometry.Sphere(radius), material)
+
+ def addCylinder(self, name, length, radius, color=None):
+ material = materialFromColor(color)
+ self.viewer[name].set_object(
+ meshcat.geometry.Cylinder(length, radius), material
+ )
+
+ def addBox(self, name, dims, color):
+ material = materialFromColor(color)
+ self.viewer[name].set_object(meshcat.geometry.Box(dims), material)
+
+ def addEllipsoid(self, name, dims, color):
+ material = materialFromColor(color)
+ self.viewer[name].set_object(meshcat.geometry.Ellipsoid(dims), material)
+
+ def applyConfiguration(self, name, placement):
+ if isinstance(placement, list) or isinstance(placement, tuple):
+ placement = np.array(placement)
+ if isinstance(placement, pin.SE3):
+ R, p = placement.rotation, placement.translation
+ T = np.r_[np.c_[R, p], [[0, 0, 0, 1]]]
+ elif isinstance(placement, np.ndarray):
+ if placement.shape == (7,): # XYZ-quat
+ R = pin.Quaternion(np.reshape(placement[3:], [4, 1])).matrix()
+ p = placement[:3]
+ T = np.r_[np.c_[R, p], [[0, 0, 0, 1]]]
+ else:
+ print("Error, np.shape of placement is not accepted")
+ return False
+ else:
+ print("Error format of placement is not accepted")
+ return False
+ self.viewer[name].set_transform(T)
+
+ def delete(self, name):
+ self.viewer[name].delete()
+
+ def __getitem__(self, name):
+ return self.viewer[name]
diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py
index 30221a7..55459a5 100644
--- a/python/ur_simple_control/visualize/visualize.py
+++ b/python/ur_simple_control/visualize/visualize.py
@@ -4,6 +4,8 @@ from collections import deque, namedtuple
import time
import copy
from pinocchio.visualize import MeshcatVisualizer
+# TODO: use wrapped meshcat visualizer to have access to more nifty plotting
+#from ur_simple_control.visualize.meshcat_viewer_wrapper.visualizer import MeshcatVisualizer
import meshcat_shapes
# tkinter stuff for later reference
@@ -165,20 +167,9 @@ def manipulatorVisualizer(args, model, collision_model, visual_model, queue):
# so we scale manually
init_target_frame = False
init_ee_frame = False
- # if we go for manipulator end-effector
#meshcat_shapes.frame(viewer["end_effector_target"], opacity=0.5)
- for geom in visual_model.geometryObjects:
- if "hand" in geom.name:
- s = geom.meshScale
- # this looks exactly correct lmao
- s *= 0.001
- geom.meshScale = s
- for geom in collision_model.geometryObjects:
- if "hand" in geom.name:
- s = geom.meshScale
- # this looks exactly correct lmao
- s *= 0.001
- geom.meshScale = s
+ # TODO: use wrapped meshcat visualizer to have access to more nifty plotting
+ #viz = MeshcatVisualizer(model=model, collision_model=collision_model, visual_model=visual_model)
viz = MeshcatVisualizer(model, collision_model, visual_model)
viz.initViewer(open=True)
viz.loadViewerModel()
@@ -220,24 +211,9 @@ def manipulatorVisualizer(args, model, collision_model, visual_model, queue):
# but they're different enough in usage to warrent a different function,
# instead of polluting the above one with ifs
def manipulatorComparisonVisualizer(args, model, collision_model, visual_model, cmd_queue, ack_queue):
- # for whatever reason the hand-e files don't have/
- # meshcat can't read scaling information.
- # so we scale manually
for geom in visual_model.geometryObjects:
if "hand" in geom.name:
- s = geom.meshScale
- # this looks exactly correct lmao
- s *= 0.001
- geom.meshScale = s
- # there has to be a way
- # if not here than elsewhere
geom.meshColor = np.array([0.2,0.2,0.2,0.2])
- for geom in collision_model.geometryObjects:
- if "hand" in geom.name:
- s = geom.meshScale
- # this looks exactly correct lmao
- s *= 0.001
- geom.meshScale = s
viz = MeshcatVisualizer(model, collision_model, visual_model)
viz.initViewer(open=True)
# load the first one
--
GitLab