From e8f155a850ea6c3bd65d8fc1261d565f2a8ccb0d Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Wed, 29 Jan 2025 14:11:07 +0100
Subject: [PATCH] new calibration for yet another demo
---
development_plan.toml | 21 +-
.../open_close_gripper.py | 18 +-
.../data/from_writing_to_handover.pickle_save | Bin 1993 -> 2766 bytes
python/examples/drawing_from_input_drawing.py | 255 ++++---
python/examples/joint_trajectory.csv | 621 ++++++++++++++++--
python/examples/path_in_pixels.csv | 621 ++++++++++++++++--
python/examples/plane_pose.pickle_save | Bin 454 -> 453 bytes
python/ur_simple_control/args_to_robot.py | 4 +
.../networking/message_specs.proto | 6 +
python/ur_simple_control/robots/heron.py | 6 +
.../ur_simple_control/robots/mobile_yumi.py | 6 +
.../robots/robotmanager_abstract.py | 49 +-
.../robots/simulated_robot.py | 30 +
.../robots/single_arm_interface.py | 0
python/ur_simple_control/robots/ur5e.py | 8 +
.../__pycache__/get_model.cpython-311.pyc | Bin 18420 -> 18548 bytes
python/ur_simple_control/util/get_model.py | 336 +++++++---
17 files changed, 1604 insertions(+), 377 deletions(-)
create mode 100644 python/ur_simple_control/args_to_robot.py
create mode 100644 python/ur_simple_control/robots/heron.py
create mode 100644 python/ur_simple_control/robots/mobile_yumi.py
create mode 100644 python/ur_simple_control/robots/simulated_robot.py
create mode 100644 python/ur_simple_control/robots/single_arm_interface.py
create mode 100644 python/ur_simple_control/robots/ur5e.py
diff --git a/development_plan.toml b/development_plan.toml
index e8aee1d..e68eb50 100644
--- a/development_plan.toml
+++ b/development_plan.toml
@@ -41,7 +41,7 @@ on top of solving the issues above, the refactoring should also:
9) args from argparse are getting exceedengly long so there should be an option to load them from a config file which can be overwritten with actual arguments.
"""
priority = 1
-deadline = 2025-01-22
+deadline = 2025-01-31
dependencies = []
purpose = """
making new features actually usable
@@ -72,7 +72,7 @@ TODO: break this up into smaller action items, this is a classic too big,
unactionable todo that we are trying to avoid by writing stuff out
"""
priority = 1
-deadline = 2025-01-22
+deadline = 2025-01-31
dependencies = []
purpose = """making typing possible - easier code writting and debugging, making library professional-grade"""
# -1 means it should be a sum over sub-items
@@ -88,8 +88,8 @@ smc does not have these. they are to be put in and leveraged.
this of course implies that using types works in the nvim setup,
that the dependencies provide type information etc.
"""
-priority = 1
-deadline = 2025-01-22
+priority = 2
+deadline = 2025-09-01
dependencies = ["create/fix type-checking workflow"]
purpose = """easier code writting and debugging, making library professional-grade"""
# -1 means it should be a sum over sub-items
@@ -105,9 +105,11 @@ having everything in one big class is already untenable.
robots with ros interfaces should have a ros-less version of functions
for the pinocchio-only option.
these can be done via ifs in those functions because 1 if is ok.
+
+various things are to be implemented with interfaces (like 2 arms, which just gives you more functions, but which only make sense if the robot has two arms). interfaces don't exist in python, but you can do multiple inheritance. these extra functions should thus be made available by inheriting stuff like TwoArmsRobot etc. have one such class for simulated robots as they all use the same getq etc.
"""
priority = 1
-deadline = 2025-01-22
+deadline = 2025-01-31
dependencies = []
purpose = """making codebase usable"""
# -1 means it should be a sum over sub-items
@@ -118,6 +120,9 @@ is_done = false
[[project_plan.action_items.action_items]]
name = "dual arms feature"
description = """
+UPDATE:
+YOU DEFINITELY HAVE TO THE JOINT JACOBIAN COMMON REFERENCE ABSOLUTE RELATIVE FRAME STUFF.
+DETAILS HAVE BEEN TAKEN A PICTIRE OF, PLUS THERE'S SEBA'S CODE ETC
robots with 2 arms need 2 targets. these can simply be 2 separate targets,
but can also be 2 transformations on 1 target - ex. in cart pulling.
this seems to best be done by having an (abstract) interface
@@ -134,7 +139,7 @@ it's impossible to say without thinking about it concretely - and
that's a part of this todo
"""
priority = 1
-deadline = 2025-01-22
+deadline = 2025-01-31
dependencies = ["robotmanager abstract class"]
purpose = """extending existing control for dual arm robots without compromising
the cleanliness of existing controllers for single arms"""
@@ -157,7 +162,7 @@ this could be implemented as an interface that's inherited or
some similar OOP concept, not sure, deciding is a part of the todo.
"""
priority = 1
-deadline = 2025-01-22
+deadline = 2025-01-31
dependencies = ["robotmanager abstract class"]
purpose = """making it easy, simple, and error-abstinent to only navigate the base"""
hours_required = 4
@@ -193,7 +198,7 @@ to install.
the most important part to nicely cut out is ros.
"""
priority = 2
-deadline = 2025-01-22
+deadline = 2025-09-01
dependencies = ["robotmanager abstract class"]
purpose = """easier shipping, enforces modularity and concern separation further,
easier maintenance. most important for not relying on ros"""
diff --git a/python/convenience_tool_box/open_close_gripper.py b/python/convenience_tool_box/open_close_gripper.py
index 32d9e36..30c1453 100644
--- a/python/convenience_tool_box/open_close_gripper.py
+++ b/python/convenience_tool_box/open_close_gripper.py
@@ -3,16 +3,22 @@ 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.managers import (
+ getMinimalArgParser,
+ ControlLoopManager,
+ RobotManager,
+)
+
def get_args():
parser = getMinimalArgParser()
- parser.description = 'open gripper, wait a few seconds, then close the gripper'
+ 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
-if __name__ == "__main__":
+
+if __name__ == "__main__":
args = get_args()
robot = RobotManager(args)
@@ -27,9 +33,9 @@ if __name__ == "__main__":
if args.save_log:
robot.log_manager.plotAllControlLoops()
- if args.visualize_manipulator:
+ if args.visualizer:
robot.killManipulatorVisualizer()
-
+
if args.save_log:
robot.log_manager.saveLog()
- #loop_manager.stopHandler(None, None)
+ # loop_manager.stopHandler(None, None)
diff --git a/python/examples/data/from_writing_to_handover.pickle_save b/python/examples/data/from_writing_to_handover.pickle_save
index 8d46773c5f1314dc4b4c551747f2f0796901e2de..7baa1bd1a4c14461433e70e54ce43c97165a6877 100644
GIT binary patch
delta 1581
zcmX@fe@>LOfo1Apu8FM9?4o(4xdoMa69a19`)q|-QhvU-Z^=DVmgGFezU?XB`khY}
z+BcM1GFI=HzCSDKCx=?l?fp+;lJ_lm`^H{DE2~}i%mRDy3&vkO-o4r1>~`|XKbPB+
znB~QfPs+N<`24)Rgw333l6i~vYyX>ct4;m&<adnf?slJdl*>E5vA1J4lm4f8bpM~T
z@y~M=?${>@C~aeYb9eunrT@2=THW05WVX^}Rp3i|wlgK^Z!Rpgm;W`#)$rY${SnM-
zvoy`Y)?BK4_AY<cN&9c&+&)475AN49O83}f4Y5X~W%8AGG7s&aTDrYz+jw=qqt%PN
z^G9CT+t$Vh=P<tB&oZw@N^Hf2{VDb<?wn-3XYVJxGiJ%IrS?xW7))K?z1jc9=J01}
zh&9i*y%X16xXXUV^_zF?!q4nCcqv$&3bkhS%9CmtQWxyaJ|yxK?Z3JI@5X?a|Fz!O
z3)JNCy#MfS|6^U<X5Yd?`)xT)ST}z<W#2zZSAc!eQu~&Fk&DCMz1e?UdHw}Ih&2L&
z+)CW43+$Io>{%xya(@5hk7~kCtsuVHTk-q50mnJ}w+9Zd-)i|_Ki}2*v!-3I>~sH1
zaAsJ)-Ov5+>R<PH$Ls|jT&-j(xo&^{RpN{y*Q@)j8~?c6`}b;pmW$8xiGOc_;-+q*
z?W<0a-S)?h-QP4x=-&RYB~zT|AAbgp8|ko(>G5BW**ofget3`j{r+Ve9q)JCI$}TN
z3fKK@N$>XW_WZG?(*MBzZ(NSStl^jJLj+`B{3yGyKXuANp8Uj9`vq%S*_6!Qfvn-U
zcbBQUW{G{wCXbje>+bCr^!fCDt<Y1jH79fL?Us=^VV`r7S@B)?`~9cgsv3fFHrX$|
zVd*Ag_kO?h#^YNSJwLJkUBi<PF1kDRZ@k^6_Q83#{U#mJ3cvfS_J?TvUBGkfJ;<7k
zQnF5lPdC_ajI(Qb{qgbsIoqn;0~{ZNtvS4`Er{p&A^Y#4toLVrd$<2}(9^0Evs3oG
znTngZcE8%ccD?!m@$+Z*3vIF7^6uXK{ciR5{r`M?V6Wyod9Ba)9s3_WEqEvV{T0ZX
z6jSRGtp&U6Ju<u5zD#|-|MSkjYv28R0Jf&Oyf~{+dXfFoLhG7i!SD8OT9U@M())`2
zvI-5g(vA1_7hLDyy?5i%eqUV&*@?xk_OE)S-8PT!jeU|wtfE};{{0uHAABBl@Gi)j
zkpB)J&K%fl&#~_N>P>52>~EP`f3!m6F4&sfTQ|RXKU}|G>Da=Gwg2Akx9iR<pZnpe
z{e}L0-}i<c-=C&_LG$$6EBix?x{fNXdAq;hWW}P!wD<O(+ok>NPwd<OZN-g571zTc
zYx;M~Ca=`lV!z-%=kadgr~5hWUpFMJxHegdRbOX8r`=-v0N0?HP{FVJ85kHG;!NM|
zzfrR>$h?N>00RR9L(D8zhlDKxfzM68Pu|EbI{5>ulzziSCztnbxAzCQ2HnYh@&%;t
zz-$YMz9Y3@eG4`~^j%~zMd&+_p}0uh{vAl)6q~1DeGEMieV2Aa^fBy$=nLe8>yy>F
zXe0Ioq=}{X8AOvCMAM{I5KRyELNr}G0e7UF_7~AF`#1bdQcd~)3*<nC5<9R1C)Ggo
zG#rBJL9ysQMAK3p?%yCy4fYUCLOBpk4@}hm+Ji#lJ(5L-pnCG9|AF<m*u4W;yfilT
z4~Tyt<K9oOp4CWttVF+nLU~ftL<UeOD_oZU0P>*82a%5;Jr0xZL-ZU#(!&JR)4GZg
ztVd+er~M6DlNM!dIS<m4;0)1o@fMPv9}qo46OF)n8V+9lwLhUW`g$eLLwitcv#f^L
K^U@3yOA-JsmItB$
delta 1147
zcmX>ndXk^Dfo1AG_KB>{9AbH;xdoMa@e}=PJ$$$FzB73D-abi8a>{z<#rwtEtyW&Y
zzuJDO%HAFJS2pZ#VLDcK?)>BZOrPT1rg6Qrul*e8#l%)@zt(t$#Sh7s`&aPazIgn`
z<NcFAa)?bn$H*c6C_0`)VAc)$l)Rm5zurG-9}&2HX~^CelP5B%dmPVSZ8Tf$vHjN-
z=Xved+~1$9d2zdn<`a8~L#O_{Ui5N*vCyP_T_3OSZ&^8MN$b<Q_6-u|J2cvh?3Fex
zm-;60a{sdKa#4X>NS3Y5=~jJl<);0SDSx~4rXAUDxk0h5dJn|1j5{YZj!!+kKk~9Z
z>-~(!`^)0pGBj&n*n4ej<I1*wzkmPQ{+o)c57>wH@}+G3b98_ItlmjCCl=YSOH`i4
zCHZoHl!-s{x0^_o1@c!c=+r)BzdMlaTA0VJ{ZUPi$_t^EEo;t>KEwWMe@99aC%60c
z{TbJs_;Pvf+8;D;Ntiz8(f+#2J^Mw(9@sBUc~`4b{$#&MPl{r)X_0*>ui4ZjiI@8~
z{W9Et8rf&xHZF|HSb226&G(e)(erNZpOotyac(!nXKS}@+3H;Oe*gVdndL=y_U>Q1
zSK-96-v{mg6wEBT5_^2VGV`A659=S>^M;C7J=*YMe{cMRBA35~_9}KiCvKH^x&N=)
z<)z8UmOX!AG)Z^GmHl$6XVgRIU)uj5;*SM8)MuLx?tbcW`OW^`|4C;i`k%LNUm#Mn
z^4qojo`NxAwsp7dw|<hBoA~areW2FM9?dsT_TM#Z`u^W#gFQpvKIxL<&-cIP@cN=1
z_!N;qj^6mS_IKF*{Tp6<mCc`cdw=d@*=MtapMVp{B1PUmty^C0w_JLD-)hNA_NR|t
z&pzyQZ~r=O&xM=kKD7U`VSW8X?VI+y*n@X;O?bZlmt5n&vK!O(i*G6BUi<I${u`%5
zDy)CpLs+)wtm$j7iI4W5QDo43U;c1^h%Wov>@D{v^RnyfJg`uBY9HVlbmeH$_x%hE
z3<tikeBK}68Wi#O(pM1QVaYs3hXWaki*~GhJoy2KzCOd^)($tOQy?`{z9fV7Bpie2
z`SBj2=RhS?&n2)PBb~rce?NdU{83H;YjD^C(XeDMM1w*NRKsbQhDqE1fHVj#hG=;3
z5u(9iCPc%7Sg3~0U=8~E22mX=q;4}F2yhMBQUTF(;08p`nZpo04xLavsR%tUgl;n)
z*zhxHsc-aOkZ&8#vwYsapleBnY~*VYpCM89w>`+mUQQ>#zBh>K&{%Vc@xYCmjX}|O
r7!NQoFf@F1{;=Oc)bnEZoSPs$3D**T*)xC~w5Mb80}fH<)KWbF=!ZTb
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index e8f8302..5e130d4 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -2,58 +2,102 @@
# TODO: make 1/beta = 5
import pinocchio as pin
import numpy as np
-import matplotlib.pyplot as plt
import matplotlib
-import copy
import argparse, argcomplete
import time
import pickle
from functools import partial
from ur_simple_control.visualize.visualize import plotFromDict
from ur_simple_control.util.draw_path import drawPath
-from ur_simple_control.dmp.dmp import getDMPArgs, DMP, NoTC,TCVelAccConstrained, followDMP
-from ur_simple_control.clik.clik import getClikArgs, getClikController, moveL, moveUntilContact, controlLoopClik, compliantMoveL, clikCartesianPathIntoJointPath
+from ur_simple_control.dmp.dmp import (
+ getDMPArgs,
+ DMP,
+ NoTC,
+ TCVelAccConstrained,
+ followDMP,
+)
+from ur_simple_control.clik.clik import (
+ getClikArgs,
+ getClikController,
+ moveL,
+ moveUntilContact,
+ controlLoopClik,
+ compliantMoveL,
+ clikCartesianPathIntoJointPath,
+)
from ur_simple_control.util.map2DPathTo3DPlane import map2DPathTo3DPlane
-from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
-from ur_simple_control.util.calib_board_hacks import getBoardCalibrationArgs, calibratePlane
+from ur_simple_control.managers import (
+ getMinimalArgParser,
+ ControlLoopManager,
+ RobotManager,
+)
+from ur_simple_control.util.calib_board_hacks import (
+ getBoardCalibrationArgs,
+ calibratePlane,
+)
from ur_simple_control.basics.basics import moveJPI
#######################################################################
# arguments #
#######################################################################
+
def getArgs():
parser = getMinimalArgParser()
parser = getClikArgs(parser)
parser = getDMPArgs(parser)
parser = getBoardCalibrationArgs(parser)
- parser.description = 'Make a drawing on screen,\
- watch the robot do it on the whiteboard.'
- parser.add_argument('--kp', type=float, \
- help="proportial control constant for position errors", \
- default=1.0)
- parser.add_argument('--mm-into-board', type=float, help="number of milimiters the path is into the board", default=3.0)
- parser.add_argument('--kv', type=float, \
- help="damping in impedance control", \
- default=0.001)
- parser.add_argument('--draw-new', action=argparse.BooleanOptionalAction, \
- help="whether draw a new picture, or use the saved path path_in_pixels.csv", default=True)
- parser.add_argument('--pick-up-marker', action=argparse.BooleanOptionalAction, \
- help="""
+ parser.description = "Make a drawing on screen,\
+ watch the robot do it on the whiteboard."
+ parser.add_argument(
+ "--kp",
+ type=float,
+ help="proportial control constant for position errors",
+ default=1.0,
+ )
+ parser.add_argument(
+ "--mm-into-board",
+ type=float,
+ help="number of milimiters the path is into the board",
+ default=3.0,
+ )
+ parser.add_argument(
+ "--kv", type=float, help="damping in impedance control", default=0.001
+ )
+ parser.add_argument(
+ "--draw-new",
+ action=argparse.BooleanOptionalAction,
+ help="whether draw a new picture, or use the saved path path_in_pixels.csv",
+ default=True,
+ )
+ parser.add_argument(
+ "--pick-up-marker",
+ action=argparse.BooleanOptionalAction,
+ help="""
whether the robot should pick up the marker.
NOTE: THIS IS FROM A PREDEFINED LOCATION.
- """, default=False)
- parser.add_argument('--find-marker-offset', action=argparse.BooleanOptionalAction, \
- help="""
+ """,
+ default=False,
+ )
+ parser.add_argument(
+ "--find-marker-offset",
+ action=argparse.BooleanOptionalAction,
+ help="""
whether you want to do find marker offset (recalculate TCP
- based on the marker""", default=True)
- parser.add_argument('--board-wiping', action=argparse.BooleanOptionalAction, \
- help="are you wiping the board (default is no because you're writing)", \
- default=False)
+ based on the marker""",
+ default=True,
+ )
+ parser.add_argument(
+ "--board-wiping",
+ action=argparse.BooleanOptionalAction,
+ help="are you wiping the board (default is no because you're writing)",
+ default=False,
+ )
argcomplete.autocomplete(parser)
args = parser.parse_args()
return args
+
def getMarker(args, robot, plane_pose):
"""
getMarker
@@ -63,7 +107,7 @@ def getMarker(args, robot, plane_pose):
if not, generate a different joint trajectory for your situation.
"""
# load traj
- file = open('./data/from_writing_to_handover.pickle_save', 'rb')
+ file = open("./data/from_writing_to_handover.pickle_save", "rb")
point_dict = pickle.load(file)
file.close()
#####################
@@ -73,9 +117,9 @@ def getMarker(args, robot, plane_pose):
tau0 = 5
# setting up array for dmp
# TODO: make the dmp general already
- for i in range(len(point_dict['qs'])):
- point_dict['qs'][i] = point_dict['qs'][i][:6]
- qs = np.array(point_dict['qs'])
+ for i in range(len(point_dict["qs"])):
+ point_dict["qs"][i] = point_dict["qs"][i][:6]
+ qs = np.array(point_dict["qs"])
followDMP(args, robot, qs, tau0)
robot.sendQd(np.zeros(robot.model.nq))
@@ -91,17 +135,17 @@ def getMarker(args, robot, plane_pose):
#############
# go back #
#############
- point_dict['qs'].reverse()
+ point_dict["qs"].reverse()
# setting up array for dmp
- qs = np.array(point_dict['qs'])
+ qs = np.array(point_dict["qs"])
followDMP(args, robot, qs, tau0)
-def findMarkerOffset(args, robot : RobotManager, plane_pose : pin.SE3):
+def findMarkerOffset(args, robot: RobotManager, plane_pose: pin.SE3):
"""
findMarkerOffset
---------------
- This relies on having the correct orientation of the plane
+ 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.
@@ -118,7 +162,7 @@ def findMarkerOffset(args, robot : RobotManager, plane_pose : pin.SE3):
speed = np.zeros(6)
speed[2] = 0.02
moveUntilContact(args, robot, speed)
- # we use the pin coordinate system because that's what's
+ # we use the pin coordinate system because that's what's
# the correct thing long term accross different robots etc
current_translation = robot.getT_w_e().translation
# i only care about the z because i'm fixing the path atm
@@ -129,7 +173,8 @@ def findMarkerOffset(args, robot : RobotManager, plane_pose : pin.SE3):
compliantMoveL(args, robot, above_starting_write_point)
return marker_offset
-def controlLoopWriting(args, robot : RobotManager, dmp, tc, i, past_data):
+
+def controlLoopWriting(args, robot: RobotManager, dmp, tc, i, past_data):
"""
controlLoopWriting
-----------------------
@@ -147,26 +192,28 @@ def controlLoopWriting(args, robot : RobotManager, dmp, tc, i, past_data):
Z = np.diag(np.array([0.0, 0.0, 1.0, 0.5, 0.5, 0.0]))
wrench = robot.getWrench()
- save_past_dict['wrench'] = wrench.copy()
+ save_past_dict["wrench"] = wrench.copy()
# rolling average
- #wrench = np.average(np.array(past_data['wrench']), axis=0)
+ # wrench = np.average(np.array(past_data['wrench']), axis=0)
# first-order low pass filtering instead
# beta is a smoothing coefficient, smaller values smooth more, has to be in [0,1]
- #wrench = args.beta * wrench + (1 - args.beta) * past_data['wrench'][-1]
- wrench = args.beta * wrench + (1 - args.beta) * np.average(np.array(past_data['wrench']), axis=0)
+ # wrench = args.beta * wrench + (1 - args.beta) * past_data['wrench'][-1]
+ wrench = args.beta * wrench + (1 - args.beta) * np.average(
+ np.array(past_data["wrench"]), axis=0
+ )
wrench = Z @ wrench
J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
- dq = robot.getQd()[:6].reshape((6,1))
- # get joint
+ dq = robot.getQd()[:6].reshape((6, 1))
+ # get joint
tau = J.T @ wrench
- tau = tau[:6].reshape((6,1))
+ tau = tau[:6].reshape((6, 1))
# compute control law:
# - feedforward the velocity and the force reading
- # - feedback the position
+ # - feedback the position
# 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
+ vel_cmd = dmp.vel + args.kp * (dmp.pos - q[:6].reshape((6, 1))) + args.alpha * tau
robot.sendQd(vel_cmd)
# tau0 is the minimum time needed for dmp
@@ -181,27 +228,30 @@ def controlLoopWriting(args, robot : RobotManager, dmp, tc, i, past_data):
# log what you said you'd log
# TODO fix the q6 situation (hide this)
- log_item['qs'] = q[:6].reshape((6,))
- log_item['dmp_qs'] = dmp.pos.reshape((6,))
- log_item['dqs'] = dq.reshape((6,))
- log_item['dmp_dqs'] = dmp.vel.reshape((6,))
- log_item['wrench'] = wrench.reshape((6,))
- log_item['tau'] = tau.reshape((6,))
+ log_item["qs"] = q[:6].reshape((6,))
+ log_item["dmp_qs"] = dmp.pos.reshape((6,))
+ log_item["dqs"] = dq.reshape((6,))
+ log_item["dmp_dqs"] = dmp.vel.reshape((6,))
+ log_item["wrench"] = wrench.reshape((6,))
+ log_item["tau"] = tau.reshape((6,))
return breakFlag, save_past_dict, log_item
-def write(args, robot : RobotManager, joint_trajectory):
+
+def write(args, robot: RobotManager, joint_trajectory):
# create DMP based on the trajectory
dmp = DMP(joint_trajectory, a_s=1.0)
if not args.temporal_coupling:
tc = NoTC()
else:
- v_max_ndarray = np.ones(robot.n_arm_joints) * robot.max_qd
- a_max_ndarray = np.ones(robot.n_arm_joints) * args.acceleration
- tc = TCVelAccConstrained(args.gamma_nominal, args.gamma_a, v_max_ndarray, a_max_ndarray, args.eps_tc)
-
- print('going to starting write position')
- dmp.step(1/500)
+ v_max_ndarray = np.ones(robot.n_arm_joints) * robot.max_qd
+ a_max_ndarray = np.ones(robot.n_arm_joints) * args.acceleration
+ tc = TCVelAccConstrained(
+ args.gamma_nominal, args.gamma_a, v_max_ndarray, a_max_ndarray, args.eps_tc
+ )
+
+ print("going to starting write position")
+ dmp.step(1 / 500)
first_q = dmp.pos.reshape((6,))
first_q = list(first_q)
first_q.append(0.0)
@@ -209,7 +259,7 @@ 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)
- # start a bit above
+ # start a bit above
go_away_from_plane_transf = pin.SE3.Identity()
go_away_from_plane_transf.translation[2] = -1 * args.mm_into_board
mtool = mtool.act(go_away_from_plane_transf)
@@ -219,20 +269,22 @@ def write(args, robot : RobotManager, joint_trajectory):
moveL(args, robot, mtool)
save_past_dict = {
- 'wrench' : np.zeros(6),
- }
+ "wrench": np.zeros(6),
+ }
# here you give it it's initial value
log_item = {
- 'qs' : np.zeros(robot.n_arm_joints),
- 'dmp_qs' : np.zeros(robot.n_arm_joints),
- 'dqs' : np.zeros(robot.n_arm_joints),
- 'dmp_dqs' : np.zeros(robot.n_arm_joints),
- 'wrench' : np.zeros(6),
- 'tau' : np.zeros(robot.n_arm_joints),
- }
- #moveJ(args, robot, dmp.pos.reshape((6,)))
+ "qs": np.zeros(robot.n_arm_joints),
+ "dmp_qs": np.zeros(robot.n_arm_joints),
+ "dqs": np.zeros(robot.n_arm_joints),
+ "dmp_dqs": np.zeros(robot.n_arm_joints),
+ "wrench": np.zeros(6),
+ "tau": np.zeros(robot.n_arm_joints),
+ }
+ # moveJ(args, robot, dmp.pos.reshape((6,)))
controlLoop = partial(controlLoopWriting, args, robot, dmp, tc)
- loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_dict, log_item)
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_dict, log_item
+ )
# and now we can actually run
loop_manager.run()
@@ -243,6 +295,7 @@ def write(args, robot : RobotManager, joint_trajectory):
goal = T_w_e.act(go_away_from_plane_transf)
compliantMoveL(args, robot, goal)
+
if __name__ == "__main__":
#######################################################################
@@ -257,39 +310,41 @@ if __name__ == "__main__":
if args.pinocchio_only:
rand_pertb = np.zeros(8)
rand_pertb[:6] = np.random.random(6) * 0.1
- robot.q = np.array([ 1.32, -1.40, -1.27, -1.157, 1.76, -0.238, 0.0, 0.0 ]) + rand_pertb
+ robot.q = (
+ np.array([1.32, -1.40, -1.27, -1.157, 1.76, -0.238, 0.0, 0.0]) + rand_pertb
+ )
robot._step()
#######################################################################
# drawing a path, making a joint trajectory for it #
#######################################################################
-
+
# draw the path on the screen
if args.draw_new:
# pure evil way to solve a bug that was pure evil
- matplotlib.use('tkagg')
+ matplotlib.use("tkagg")
pixel_path = drawPath(args)
- matplotlib.use('qtagg')
+ matplotlib.use("qtagg")
else:
if not args.board_wiping:
- pixel_path_file_path = './path_in_pixels.csv'
- pixel_path = np.genfromtxt(pixel_path_file_path, delimiter=',')
+ pixel_path_file_path = "./path_in_pixels.csv"
+ pixel_path = np.genfromtxt(pixel_path_file_path, delimiter=",")
else:
- pixel_path_file_path = './wiping_path.csv_save'
- pixel_path = np.genfromtxt(pixel_path_file_path, delimiter=',')
+ pixel_path_file_path = "./wiping_path.csv_save"
+ pixel_path = np.genfromtxt(pixel_path_file_path, delimiter=",")
# do calibration if specified
if args.calibration:
- plane_pose, q_init = \
- calibratePlane(args, robot, args.board_width, args.board_height, \
- args.n_calibration_tests)
+ plane_pose, q_init = calibratePlane(
+ args, robot, args.board_width, args.board_height, args.n_calibration_tests
+ )
print("finished calibration")
else:
print("using existing plane calibration")
- file = open('./plane_pose.pickle_save', 'rb')
+ file = open("./plane_pose.pickle_save", "rb")
plane_calib_dict = pickle.load(file)
file.close()
- plane_pose = plane_calib_dict['plane_top_left_pose']
- q_init = plane_calib_dict['q_init']
+ plane_pose = plane_calib_dict["plane_top_left_pose"]
+ q_init = plane_calib_dict["q_init"]
# stupid fix dw about this it's supposed to be 0.0 on the gripper
# because we don't use that actually
q_init[-1] = 0.0
@@ -297,9 +352,8 @@ if __name__ == "__main__":
# make the path 3D
path_points_3D = map2DPathTo3DPlane(pixel_path, args.board_width, args.board_height)
- # TODO: fix thi function
if args.pick_up_marker:
- #raise NotImplementedError("sorry")
+ # raise NotImplementedError("sorry")
getMarker(args, robot, None)
# marker moves between runs, i change markers etc,
@@ -309,14 +363,18 @@ if __name__ == "__main__":
# find the marker offset
marker_offset = findMarkerOffset(args, robot, plane_pose)
robot.sendQd(np.zeros(6))
- print('marker_offset', marker_offset)
+ print("marker_offset", marker_offset)
# we're going in a bit deeper
- path_points_3D = path_points_3D + np.array([0.0, 0.0, -1 * marker_offset + args.mm_into_board])
+ path_points_3D = path_points_3D + np.array(
+ [0.0, 0.0, -1 * marker_offset + args.mm_into_board]
+ )
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 = path + np.array([0.0, 0.0, -0.1503])
marker_offset = 0.066
- path_points_3D = path_points_3D + np.array([0.0, 0.0, -1 * marker_offset + args.mm_into_board])
+ path_points_3D = path_points_3D + np.array(
+ [0.0, 0.0, -1 * marker_offset + args.mm_into_board]
+ )
# create a joint space trajectory based on the 3D path
if args.draw_new or args.calibration or args.find_marker_offset:
@@ -328,14 +386,15 @@ if __name__ == "__main__":
if args.viz_test_path:
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!
"""
- )
+ )
clikController = getClikController(args, robot)
- joint_trajectory = clikCartesianPathIntoJointPath(args, robot, path, \
- clikController, q_init, plane_pose)
+ joint_trajectory = clikCartesianPathIntoJointPath(
+ args, robot, path, clikController, q_init, plane_pose
+ )
if args.viz_test_path:
answer = input("did the movement of the manipulator look reasonable? [Y/n]")
if not (answer == "Y" or answer == "y"):
@@ -346,9 +405,9 @@ if __name__ == "__main__":
else:
answer = True
else:
- joint_trajectory_file_path = './joint_trajectory.csv'
- joint_trajectory = np.genfromtxt(joint_trajectory_file_path, delimiter=',')
-
+ joint_trajectory_file_path = "./joint_trajectory.csv"
+ joint_trajectory = np.genfromtxt(joint_trajectory_file_path, delimiter=",")
+
if answer:
write(args, robot, joint_trajectory)
@@ -363,5 +422,3 @@ if __name__ == "__main__":
if args.save_log:
robot.log_manager.plotAllControlLoops()
-
-
diff --git a/python/examples/joint_trajectory.csv b/python/examples/joint_trajectory.csv
index c91cde2..f2401c0 100644
--- a/python/examples/joint_trajectory.csv
+++ b/python/examples/joint_trajectory.csv
@@ -1,58 +1,563 @@
-0.00000,0.45874,-1.08962,-1.51206,-1.38620,2.51445,-0.74052
-0.17544,0.45868,-1.08964,-1.51205,-1.38626,2.51449,-0.74058
-0.35088,0.45863,-1.08965,-1.51202,-1.38631,2.51452,-0.74064
-0.52632,0.45858,-1.08968,-1.51198,-1.38637,2.51455,-0.74069
-0.70175,0.45686,-1.09130,-1.50949,-1.38873,2.51552,-0.74237
-0.87719,0.45058,-1.10155,-1.49463,-1.39887,2.51904,-0.74852
-1.05263,0.44612,-1.11303,-1.47853,-1.40748,2.52153,-0.75295
-1.22807,0.44362,-1.12180,-1.46649,-1.41303,2.52293,-0.75549
-1.40351,0.44026,-1.13670,-1.44704,-1.42079,2.52487,-0.75917
-1.57895,0.43214,-1.16746,-1.41083,-1.43467,2.52992,-0.76930
-1.75439,0.42839,-1.18095,-1.39531,-1.44074,2.53228,-0.77417
-1.92982,0.42411,-1.19552,-1.37897,-1.44719,2.53499,-0.77985
-2.10526,0.41083,-1.23371,-1.33875,-1.46409,2.54336,-0.79793
-2.28070,0.40583,-1.24716,-1.32481,-1.47033,2.54649,-0.80489
-2.45614,0.39519,-1.27190,-1.30087,-1.48194,2.55307,-0.81990
-2.63158,0.38707,-1.28820,-1.28629,-1.48988,2.55803,-0.83154
-2.80702,0.38140,-1.29800,-1.27840,-1.49482,2.56145,-0.83976
-2.98246,0.37530,-1.30762,-1.27132,-1.49974,2.56510,-0.84870
-3.15789,0.36691,-1.31857,-1.26498,-1.50553,2.57007,-0.86114
-3.33333,0.35977,-1.32735,-1.26050,-1.51023,2.57424,-0.87187
-3.50877,0.35378,-1.33423,-1.25755,-1.51397,2.57770,-0.88098
-3.68421,0.35062,-1.33768,-1.25627,-1.51588,2.57950,-0.88582
-3.85965,0.34475,-1.34349,-1.25486,-1.51913,2.58284,-0.89488
-4.03509,0.33688,-1.34930,-1.25589,-1.52268,2.58725,-0.90716
-4.21053,0.32958,-1.35313,-1.25916,-1.52537,2.59128,-0.91869
-4.38596,0.32489,-1.35510,-1.26201,-1.52692,2.59384,-0.92618
-4.56140,0.32055,-1.35637,-1.26544,-1.52815,2.59619,-0.93315
-4.73684,0.31739,-1.35676,-1.26869,-1.52887,2.59788,-0.93826
-4.91228,0.30875,-1.35517,-1.28132,-1.52988,2.60246,-0.95237
-5.08772,0.29881,-1.34908,-1.30161,-1.52978,2.60762,-0.96883
-5.26316,0.29288,-1.34447,-1.31500,-1.52956,2.61064,-0.97878
-5.43860,0.28492,-1.33645,-1.33529,-1.52889,2.61462,-0.99227
-5.61404,0.27574,-1.32443,-1.36209,-1.52773,2.61913,-1.00807
-5.78947,0.27105,-1.31721,-1.37700,-1.52708,2.62138,-1.01621
-5.96491,0.26772,-1.31006,-1.38999,-1.52627,2.62297,-1.02203
-6.14035,0.26371,-1.29429,-1.41418,-1.52396,2.62487,-1.02907
-6.31579,0.26195,-1.28166,-1.43150,-1.52193,2.62570,-1.03215
-6.49123,0.26203,-1.25338,-1.46478,-1.51674,2.62568,-1.03192
-6.66667,0.26378,-1.23798,-1.48068,-1.51351,2.62488,-1.02876
-6.84211,0.26788,-1.22076,-1.49587,-1.50920,2.62296,-1.02146
-7.01754,0.27556,-1.19947,-1.51183,-1.50286,2.61931,-1.00795
-7.19298,0.28437,-1.18268,-1.52173,-1.49658,2.61502,-0.99267
-7.36842,0.29389,-1.16823,-1.52857,-1.49020,2.61026,-0.97642
-7.54386,0.31292,-1.14531,-1.53637,-1.47819,2.60042,-0.94472
-7.71930,0.31820,-1.13976,-1.53792,-1.47482,2.59761,-0.93609
-7.89474,0.32985,-1.13120,-1.53730,-1.46807,2.59129,-0.91737
-8.07018,0.33983,-1.12633,-1.53425,-1.46265,2.58576,-0.90162
-8.24561,0.35186,-1.12401,-1.52673,-1.45678,2.57894,-0.88302
-8.42105,0.37319,-1.12681,-1.50568,-1.44819,2.56649,-0.85105
-8.59649,0.38315,-1.12948,-1.49441,-1.44468,2.56051,-0.83653
-8.77193,0.39544,-1.13475,-1.47806,-1.44118,2.55301,-0.81898
-8.94737,0.40970,-1.14329,-1.45598,-1.43833,2.54413,-0.79913
-9.12281,0.41767,-1.14866,-1.44282,-1.43718,2.53909,-0.78826
-9.29825,0.42642,-1.15734,-1.42472,-1.43699,2.53349,-0.77652
-9.47368,0.43304,-1.16672,-1.40726,-1.43793,2.52921,-0.76778
-9.64912,0.43800,-1.17945,-1.38656,-1.44064,2.52598,-0.76133
-9.82456,0.43939,-1.18667,-1.37587,-1.44265,2.52506,-0.75953
-10.00000,0.43940,-1.18671,-1.37581,-1.44267,2.52506,-0.75952
+0.00000,0.78295,-1.13575,-1.51349,-1.07472,2.27753,-0.40069
+0.02669,0.76500,-1.13780,-1.52114,-1.07109,2.29259,-0.41171
+0.05338,0.74778,-1.14049,-1.52837,-1.06810,2.30696,-0.42320
+0.08007,0.72877,-1.14414,-1.53623,-1.06523,2.32272,-0.43675
+0.10676,0.70673,-1.14912,-1.54515,-1.06239,2.34084,-0.45337
+0.13345,0.68745,-1.15349,-1.55369,-1.05991,2.35656,-0.46860
+0.16014,0.67447,-1.15652,-1.55965,-1.05832,2.36707,-0.47920
+0.18683,0.66159,-1.16003,-1.56506,-1.05703,2.37744,-0.48999
+0.21352,0.63371,-1.17002,-1.57409,-1.05556,2.39965,-0.51430
+0.24021,0.61938,-1.17525,-1.57906,-1.05489,2.41095,-0.52727
+0.26690,0.60982,-1.17873,-1.58257,-1.05443,2.41844,-0.53611
+0.29359,0.59596,-1.18436,-1.58711,-1.05406,2.42923,-0.54921
+0.32028,0.58547,-1.18861,-1.59079,-1.05377,2.43733,-0.55935
+0.34698,0.57711,-1.19215,-1.59364,-1.05362,2.44376,-0.56757
+0.37367,0.56816,-1.19626,-1.59635,-1.05362,2.45060,-0.57651
+0.40036,0.55760,-1.20085,-1.60016,-1.05345,2.45861,-0.58724
+0.42705,0.54930,-1.20415,-1.60381,-1.05313,2.46487,-0.59583
+0.45374,0.54301,-1.20657,-1.60680,-1.05284,2.46959,-0.60242
+0.48043,0.53618,-1.20868,-1.61094,-1.05224,2.47469,-0.60966
+0.50712,0.53193,-1.21007,-1.61345,-1.05190,2.47785,-0.61422
+0.53381,0.52058,-1.21360,-1.62065,-1.05085,2.48623,-0.62655
+0.56050,0.51293,-1.21571,-1.62609,-1.05000,2.49185,-0.63502
+0.58719,0.50599,-1.21755,-1.63123,-1.04919,2.49690,-0.64281
+0.61388,0.50014,-1.21868,-1.63630,-1.04828,2.50115,-0.64946
+0.64057,0.49499,-1.21946,-1.64114,-1.04737,2.50486,-0.65536
+0.66726,0.49030,-1.21981,-1.64614,-1.04636,2.50823,-0.66079
+0.69395,0.48719,-1.22015,-1.64933,-1.04574,2.51046,-0.66442
+0.72064,0.48185,-1.22083,-1.65469,-1.04474,2.51426,-0.67069
+0.74733,0.47440,-1.22189,-1.66215,-1.04338,2.51953,-0.67956
+0.77402,0.46814,-1.22305,-1.66809,-1.04239,2.52395,-0.68712
+0.80071,0.45976,-1.22482,-1.67585,-1.04119,2.52980,-0.69737
+0.82740,0.45644,-1.22539,-1.67916,-1.04066,2.53210,-0.70149
+0.85409,0.45302,-1.22622,-1.68224,-1.04023,2.53447,-0.70575
+0.88078,0.44896,-1.22736,-1.68569,-1.03981,2.53727,-0.71085
+0.90747,0.44531,-1.22835,-1.68887,-1.03941,2.53977,-0.71547
+0.93416,0.44183,-1.22950,-1.69164,-1.03913,2.54215,-0.71990
+0.96085,0.43578,-1.23187,-1.69598,-1.03882,2.54627,-0.72769
+0.98754,0.43073,-1.23404,-1.69941,-1.03866,2.54968,-0.73427
+1.01423,0.42523,-1.23675,-1.70269,-1.03864,2.55338,-0.74150
+1.04093,0.42068,-1.23917,-1.70519,-1.03872,2.55641,-0.74755
+1.06762,0.41667,-1.24130,-1.70745,-1.03879,2.55908,-0.75292
+1.09431,0.41468,-1.24242,-1.70848,-1.03886,2.56039,-0.75560
+1.12100,0.41221,-1.24383,-1.70977,-1.03895,2.56202,-0.75894
+1.14769,0.40972,-1.24526,-1.71107,-1.03905,2.56366,-0.76234
+1.17438,0.40630,-1.24729,-1.71280,-1.03920,2.56590,-0.76702
+1.20107,0.40289,-1.24957,-1.71417,-1.03947,2.56813,-0.77173
+1.22776,0.39899,-1.25211,-1.71589,-1.03974,2.57066,-0.77714
+1.25445,0.38703,-1.25933,-1.72231,-1.04026,2.57834,-0.79401
+1.28114,0.37845,-1.26382,-1.72816,-1.04033,2.58376,-0.80637
+1.30783,0.37052,-1.26770,-1.73413,-1.04029,2.58871,-0.81797
+1.33452,0.36728,-1.26926,-1.73666,-1.04027,2.59072,-0.82277
+1.36121,0.36624,-1.26971,-1.73754,-1.04025,2.59136,-0.82431
+1.38790,0.36467,-1.27029,-1.73902,-1.04017,2.59233,-0.82664
+1.41459,0.36414,-1.27047,-1.73953,-1.04014,2.59265,-0.82744
+1.44128,0.36411,-1.27048,-1.73956,-1.04014,2.59267,-0.82748
+1.46797,0.36409,-1.27050,-1.73956,-1.04015,2.59268,-0.82751
+1.49466,0.36408,-1.27053,-1.73954,-1.04016,2.59269,-0.82753
+1.52135,0.36406,-1.27058,-1.73950,-1.04017,2.59270,-0.82755
+1.54804,0.36393,-1.28387,-1.72040,-1.04623,2.59281,-0.82787
+1.57473,0.36733,-1.30667,-1.68164,-1.05807,2.59076,-0.82302
+1.60142,0.36909,-1.31603,-1.66493,-1.06329,2.58969,-0.82051
+1.62811,0.37203,-1.32690,-1.64397,-1.06980,2.58789,-0.81628
+1.65480,0.37481,-1.33497,-1.62737,-1.07496,2.58618,-0.81231
+1.68149,0.37823,-1.34470,-1.60713,-1.08135,2.58405,-0.80743
+1.70819,0.38129,-1.35462,-1.58707,-1.08786,2.58215,-0.80312
+1.73488,0.38417,-1.36653,-1.56404,-1.09557,2.58035,-0.79910
+1.76157,0.38620,-1.37464,-1.54813,-1.10100,2.57908,-0.79627
+1.78826,0.38819,-1.38330,-1.53134,-1.10682,2.57783,-0.79353
+1.81495,0.39031,-1.39262,-1.51319,-1.11320,2.57650,-0.79062
+1.84164,0.39251,-1.40142,-1.49565,-1.11942,2.57511,-0.78762
+1.86833,0.39397,-1.40759,-1.48341,-1.12382,2.57418,-0.78562
+1.89502,0.39577,-1.41545,-1.46786,-1.12947,2.57304,-0.78319
+1.92171,0.39717,-1.42175,-1.45540,-1.13405,2.57215,-0.78131
+1.94840,0.39902,-1.43087,-1.43759,-1.14066,2.57098,-0.77883
+1.97509,0.40057,-1.43962,-1.42078,-1.14701,2.56999,-0.77678
+2.00178,0.40196,-1.44686,-1.40659,-1.15240,2.56910,-0.77493
+2.02847,0.40335,-1.45461,-1.39154,-1.15817,2.56821,-0.77310
+2.05516,0.40463,-1.46080,-1.37919,-1.16293,2.56740,-0.77142
+2.08185,0.40512,-1.46306,-1.37463,-1.16469,2.56708,-0.77078
+2.10854,0.40546,-1.46470,-1.37133,-1.16597,2.56686,-0.77032
+2.13523,0.40554,-1.46511,-1.37053,-1.16629,2.56681,-0.77022
+2.16192,0.40566,-1.46573,-1.36928,-1.16677,2.56673,-0.77006
+2.18861,0.40700,-1.47190,-1.35679,-1.17162,2.56587,-0.76830
+2.21530,0.40914,-1.48134,-1.33747,-1.17917,2.56449,-0.76549
+2.24199,0.41085,-1.48754,-1.32421,-1.18435,2.56338,-0.76325
+2.26868,0.41248,-1.49351,-1.31145,-1.18937,2.56233,-0.76112
+2.29537,0.41410,-1.49952,-1.29858,-1.19447,2.56127,-0.75900
+2.32206,0.41705,-1.50945,-1.27681,-1.20311,2.55934,-0.75516
+2.34875,0.41874,-1.51548,-1.26370,-1.20837,2.55823,-0.75297
+2.37544,0.42040,-1.52161,-1.25045,-1.21372,2.55714,-0.75083
+2.40214,0.42143,-1.52555,-1.24198,-1.21716,2.55647,-0.74952
+2.42883,0.42265,-1.52970,-1.23279,-1.22089,2.55566,-0.74795
+2.45552,0.42310,-1.53132,-1.22925,-1.22234,2.55536,-0.74737
+2.48221,0.42455,-1.53695,-1.21711,-1.22731,2.55441,-0.74553
+2.50890,0.42535,-1.54034,-1.20991,-1.23028,2.55388,-0.74451
+2.53559,0.42593,-1.54274,-1.20479,-1.23240,2.55350,-0.74379
+2.56228,0.42632,-1.54446,-1.20114,-1.23392,2.55324,-0.74329
+2.58897,0.42670,-1.54621,-1.19747,-1.23544,2.55298,-0.74281
+2.61566,0.42704,-1.54759,-1.19450,-1.23668,2.55276,-0.74238
+2.64235,0.42783,-1.55104,-1.18718,-1.23973,2.55224,-0.74139
+2.66904,0.42858,-1.55461,-1.17970,-1.24286,2.55174,-0.74045
+2.69573,0.42967,-1.55957,-1.16923,-1.24724,2.55101,-0.73909
+2.72242,0.43042,-1.56319,-1.16165,-1.25044,2.55052,-0.73817
+2.74911,0.43062,-1.56431,-1.15935,-1.25141,2.55039,-0.73791
+2.77580,0.43092,-1.56573,-1.15637,-1.25267,2.55019,-0.73754
+2.80249,0.43107,-1.56644,-1.15486,-1.25331,2.55009,-0.73735
+2.82918,0.43146,-1.56824,-1.15106,-1.25492,2.54983,-0.73688
+2.85587,0.43162,-1.56898,-1.14946,-1.25559,2.54972,-0.73667
+2.88256,0.43179,-1.56969,-1.14796,-1.25623,2.54961,-0.73646
+2.90925,0.43180,-1.56970,-1.14790,-1.25625,2.54960,-0.73645
+2.93594,0.43182,-1.56969,-1.14790,-1.25625,2.54959,-0.73642
+2.96263,0.43184,-1.56965,-1.14793,-1.25624,2.54957,-0.73639
+2.98932,0.43548,-1.56182,-1.15545,-1.25263,2.54711,-0.73163
+3.01601,0.43881,-1.55364,-1.16424,-1.24846,2.54484,-0.72728
+3.04270,0.44158,-1.54573,-1.17353,-1.24412,2.54294,-0.72367
+3.06940,0.44384,-1.53841,-1.18267,-1.23988,2.54139,-0.72073
+3.09609,0.44631,-1.53007,-1.19327,-1.23498,2.53969,-0.71751
+3.12278,0.44898,-1.51954,-1.20741,-1.22853,2.53784,-0.71403
+3.14947,0.45358,-1.49851,-1.23678,-1.21530,2.53464,-0.70804
+3.17616,0.45586,-1.48870,-1.25030,-1.20918,2.53304,-0.70509
+3.20285,0.45733,-1.48189,-1.25985,-1.20489,2.53201,-0.70318
+3.22954,0.45897,-1.47365,-1.27160,-1.19964,2.53086,-0.70106
+3.25623,0.46108,-1.46396,-1.28512,-1.19360,2.52937,-0.69835
+3.28292,0.46419,-1.45049,-1.30361,-1.18535,2.52718,-0.69439
+3.30961,0.47077,-1.42542,-1.33678,-1.17056,2.52253,-0.68613
+3.33630,0.47307,-1.41640,-1.34884,-1.16520,2.52089,-0.68325
+3.36299,0.47638,-1.40176,-1.36892,-1.15642,2.51853,-0.67913
+3.38968,0.47989,-1.38671,-1.38936,-1.14752,2.51601,-0.67479
+3.41637,0.48396,-1.37118,-1.40977,-1.13863,2.51309,-0.66981
+3.44306,0.48629,-1.36272,-1.42075,-1.13385,2.51141,-0.66699
+3.46975,0.48903,-1.35289,-1.43344,-1.12834,2.50944,-0.66370
+3.49644,0.49184,-1.34346,-1.44535,-1.12316,2.50741,-0.66034
+3.52313,0.49660,-1.32755,-1.46537,-1.11453,2.50396,-0.65468
+3.54982,0.49959,-1.31776,-1.47762,-1.10926,2.50178,-0.65116
+3.57651,0.50255,-1.30709,-1.49133,-1.10346,2.49962,-0.64769
+3.60320,0.50532,-1.29642,-1.50524,-1.09763,2.49759,-0.64445
+3.62989,0.50762,-1.28672,-1.51815,-1.09229,2.49591,-0.64177
+3.65658,0.51143,-1.27160,-1.53789,-1.08418,2.49310,-0.63736
+3.68327,0.51488,-1.25967,-1.55288,-1.07799,2.49056,-0.63341
+3.70996,0.51788,-1.24949,-1.56561,-1.07275,2.48834,-0.63001
+3.73665,0.52013,-1.24215,-1.57465,-1.06904,2.48668,-0.62746
+3.76335,0.52249,-1.23429,-1.58441,-1.06506,2.48493,-0.62481
+3.79004,0.52554,-1.22367,-1.59768,-1.05971,2.48266,-0.62139
+3.81673,0.52859,-1.21352,-1.61022,-1.05467,2.48039,-0.61801
+3.84342,0.53220,-1.20293,-1.62277,-1.04959,2.47769,-0.61403
+3.87011,0.53570,-1.19322,-1.63406,-1.04502,2.47507,-0.61020
+3.89680,0.54038,-1.18105,-1.64787,-1.03943,2.47157,-0.60514
+3.92349,0.54282,-1.17495,-1.65470,-1.03666,2.46973,-0.60252
+3.95018,0.54632,-1.16694,-1.66335,-1.03312,2.46710,-0.59879
+3.97687,0.55011,-1.15883,-1.67184,-1.02963,2.46424,-0.59477
+4.00356,0.55477,-1.14925,-1.68170,-1.02557,2.46072,-0.58989
+4.03025,0.55959,-1.13927,-1.69204,-1.02135,2.45706,-0.58487
+4.05694,0.56386,-1.12941,-1.70276,-1.01709,2.45381,-0.58046
+4.08363,0.56832,-1.11807,-1.71555,-1.01214,2.45040,-0.57588
+4.11032,0.57235,-1.10522,-1.73102,-1.00637,2.44730,-0.57176
+4.13701,0.57375,-1.10047,-1.73682,-1.00422,2.44622,-0.57032
+4.16370,0.57505,-1.09576,-1.74266,-1.00208,2.44522,-0.56899
+4.19039,0.57615,-1.09218,-1.74697,-1.00050,2.44437,-0.56788
+4.21708,0.57736,-1.08857,-1.75122,-0.99893,2.44344,-0.56666
+4.24377,0.57815,-1.08635,-1.75378,-0.99798,2.44283,-0.56586
+4.27046,0.57916,-1.08351,-1.75706,-0.99677,2.44205,-0.56484
+4.29715,0.57966,-1.08211,-1.75868,-0.99617,2.44166,-0.56434
+4.32384,0.58016,-1.08070,-1.76031,-0.99558,2.44128,-0.56384
+4.35053,0.58068,-1.07925,-1.76198,-0.99496,2.44088,-0.56332
+4.37722,0.58152,-1.07701,-1.76451,-0.99402,2.44023,-0.56248
+4.40391,0.58205,-1.07560,-1.76612,-0.99343,2.43982,-0.56195
+4.43060,0.58200,-1.07563,-1.76612,-0.99344,2.43986,-0.56200
+4.45730,0.57606,-1.08020,-1.76525,-0.99417,2.44440,-0.56785
+4.48399,0.56139,-1.09411,-1.75930,-0.99748,2.45558,-0.58260
+4.51068,0.54657,-1.11041,-1.75016,-1.00216,2.46676,-0.59794
+4.53737,0.53585,-1.12284,-1.74277,-1.00597,2.47478,-0.60932
+4.56406,0.52728,-1.13317,-1.73639,-1.00927,2.48116,-0.61860
+4.59075,0.51815,-1.14458,-1.72913,-1.01301,2.48789,-0.62864
+4.61744,0.50664,-1.16041,-1.71809,-1.01845,2.49633,-0.64157
+4.64413,0.48370,-1.19521,-1.69217,-1.03086,2.51289,-0.66823
+4.67082,0.46985,-1.21855,-1.67349,-1.03968,2.52273,-0.68492
+4.69751,0.46333,-1.23032,-1.66361,-1.04430,2.52731,-0.69295
+4.72420,0.45703,-1.24191,-1.65387,-1.04885,2.53172,-0.70082
+4.75089,0.45257,-1.25032,-1.64670,-1.05220,2.53482,-0.70643
+4.77758,0.44546,-1.26394,-1.63511,-1.05759,2.53973,-0.71550
+4.80427,0.43880,-1.27786,-1.62258,-1.06328,2.54430,-0.72413
+4.83096,0.43229,-1.29148,-1.61041,-1.06886,2.54873,-0.73265
+4.85765,0.42837,-1.29990,-1.60277,-1.07236,2.55138,-0.73785
+4.88434,0.42163,-1.31448,-1.58963,-1.07839,2.55591,-0.74688
+4.91103,0.41552,-1.32800,-1.57736,-1.08403,2.55999,-0.75517
+4.93772,0.41048,-1.33945,-1.56682,-1.08887,2.56333,-0.76209
+4.96441,0.40694,-1.34778,-1.55901,-1.09245,2.56566,-0.76700
+4.99110,0.39983,-1.36573,-1.54154,-1.10029,2.57031,-0.77695
+5.01779,0.39584,-1.37639,-1.53084,-1.10508,2.57291,-0.78262
+5.04448,0.39312,-1.38377,-1.52337,-1.10843,2.57467,-0.78651
+5.07117,0.39006,-1.39214,-1.51487,-1.11225,2.57664,-0.79090
+5.09786,0.38854,-1.39646,-1.51041,-1.11424,2.57761,-0.79310
+5.12456,0.38667,-1.40160,-1.50519,-1.11660,2.57881,-0.79581
+5.15125,0.38516,-1.40591,-1.50073,-1.11859,2.57977,-0.79800
+5.17794,0.38178,-1.41519,-1.49138,-1.12282,2.58192,-0.80294
+5.20463,0.37912,-1.42239,-1.48420,-1.12610,2.58361,-0.80685
+5.23132,0.37575,-1.43180,-1.47469,-1.13042,2.58574,-0.81183
+5.25801,0.37248,-1.44121,-1.46503,-1.13479,2.58779,-0.81671
+5.28470,0.37103,-1.44533,-1.46082,-1.13671,2.58870,-0.81887
+5.31139,0.36883,-1.45157,-1.45449,-1.13961,2.59007,-0.82218
+5.33808,0.36738,-1.45566,-1.45035,-1.14151,2.59097,-0.82437
+5.36477,0.36595,-1.45989,-1.44595,-1.14352,2.59186,-0.82653
+5.39146,0.36453,-1.46406,-1.44165,-1.14548,2.59274,-0.82868
+5.41815,0.36313,-1.46816,-1.43742,-1.14742,2.59361,-0.83082
+5.44484,0.36099,-1.47439,-1.43104,-1.15034,2.59492,-0.83408
+5.47153,0.35958,-1.47849,-1.42684,-1.15227,2.59579,-0.83624
+5.49822,0.35745,-1.48471,-1.42049,-1.15520,2.59709,-0.83951
+5.52491,0.35640,-1.48779,-1.41733,-1.15666,2.59773,-0.84113
+5.55160,0.35495,-1.49184,-1.41330,-1.15854,2.59861,-0.84337
+5.57829,0.35345,-1.49591,-1.40935,-1.16041,2.59953,-0.84570
+5.60498,0.35271,-1.49794,-1.40737,-1.16134,2.59998,-0.84685
+5.63167,0.35197,-1.49997,-1.40537,-1.16228,2.60042,-0.84799
+5.65836,0.35160,-1.50099,-1.40437,-1.16276,2.60064,-0.84856
+5.68505,0.35124,-1.50201,-1.40336,-1.16323,2.60087,-0.84913
+5.71174,0.35122,-1.50205,-1.40332,-1.16325,2.60087,-0.84916
+5.73843,0.35121,-1.50209,-1.40328,-1.16327,2.60088,-0.84918
+5.76512,0.35119,-1.50213,-1.40324,-1.16329,2.60089,-0.84920
+5.79181,0.35118,-1.50217,-1.40320,-1.16331,2.60090,-0.84923
+5.81851,0.35116,-1.50221,-1.40316,-1.16333,2.60091,-0.84925
+5.84520,0.35115,-1.50224,-1.40313,-1.16334,2.60092,-0.84927
+5.87189,0.35114,-1.50228,-1.40309,-1.16336,2.60093,-0.84929
+5.89858,0.35113,-1.50231,-1.40306,-1.16337,2.60093,-0.84930
+5.92527,0.35112,-1.50234,-1.40303,-1.16339,2.60094,-0.84931
+5.95196,0.35112,-1.50236,-1.40300,-1.16340,2.60094,-0.84932
+5.97865,0.35112,-1.50236,-1.40298,-1.16341,2.60093,-0.84931
+6.00534,0.35113,-1.50235,-1.40297,-1.16341,2.60093,-0.84930
+6.03203,0.35129,-1.50216,-1.40298,-1.16339,2.60083,-0.84905
+6.05872,0.36641,-1.47926,-1.41253,-1.15734,2.59164,-0.82624
+6.08541,0.37094,-1.47256,-1.41546,-1.15541,2.58883,-0.81951
+6.11210,0.37875,-1.46070,-1.42130,-1.15173,2.58395,-0.80804
+6.13879,0.38821,-1.44682,-1.42802,-1.14737,2.57795,-0.79436
+6.16548,0.39402,-1.43829,-1.43237,-1.14458,2.57422,-0.78607
+6.19217,0.40248,-1.42687,-1.43734,-1.14107,2.56875,-0.77418
+6.21886,0.42735,-1.39363,-1.45298,-1.13057,2.55229,-0.74039
+6.24555,0.46348,-1.34433,-1.48139,-1.11350,2.52749,-0.69412
+6.27224,0.47184,-1.33369,-1.48749,-1.10972,2.52161,-0.68386
+6.29893,0.48703,-1.31600,-1.49631,-1.10391,2.51081,-0.66566
+6.32562,0.50730,-1.29446,-1.50577,-1.09728,2.49618,-0.64222
+6.35231,0.51761,-1.28425,-1.50987,-1.09422,2.48863,-0.63064
+6.37900,0.53545,-1.26743,-1.51615,-1.08943,2.47543,-0.61114
+6.40569,0.54867,-1.25577,-1.52008,-1.08624,2.46553,-0.59710
+6.43238,0.56028,-1.24665,-1.52208,-1.08407,2.45676,-0.58507
+6.45907,0.56520,-1.24318,-1.52240,-1.08336,2.45303,-0.58005
+6.48577,0.56771,-1.24150,-1.52245,-1.08304,2.45112,-0.57751
+6.51246,0.56896,-1.24068,-1.52244,-1.08290,2.45017,-0.57624
+6.53915,0.58370,-1.22968,-1.52473,-1.08043,2.43887,-0.56156
+6.56584,0.59087,-1.22428,-1.52612,-1.07915,2.43334,-0.55456
+6.59253,0.60266,-1.21546,-1.52853,-1.07706,2.42420,-0.54324
+6.61922,0.61018,-1.20984,-1.53021,-1.07569,2.41833,-0.53614
+6.64591,0.61478,-1.20653,-1.53110,-1.07492,2.41473,-0.53184
+6.67260,0.62020,-1.20254,-1.53233,-1.07396,2.41047,-0.52682
+6.69929,0.62257,-1.20084,-1.53283,-1.07356,2.40861,-0.52464
+6.72598,0.62598,-1.19840,-1.53354,-1.07299,2.40592,-0.52152
+6.75267,0.62973,-1.19584,-1.53415,-1.07243,2.40297,-0.51811
+6.77936,0.63463,-1.19257,-1.53484,-1.07176,2.39909,-0.51368
+6.80605,0.63960,-1.18935,-1.53545,-1.07112,2.39515,-0.50923
+6.83274,0.64464,-1.18617,-1.53598,-1.07053,2.39114,-0.50476
+6.85943,0.64980,-1.18298,-1.53645,-1.06996,2.38703,-0.50023
+6.88612,0.65496,-1.17986,-1.53686,-1.06942,2.38291,-0.49572
+6.91281,0.65869,-1.17760,-1.53719,-1.06903,2.37992,-0.49249
+6.93950,0.66174,-1.17586,-1.53732,-1.06876,2.37748,-0.48987
+6.96619,0.66554,-1.17366,-1.53753,-1.06842,2.37443,-0.48662
+6.99288,0.66936,-1.17143,-1.53780,-1.06807,2.37135,-0.48337
+7.01957,0.67206,-1.16988,-1.53797,-1.06782,2.36918,-0.48109
+7.04626,0.67364,-1.16899,-1.53803,-1.06770,2.36790,-0.47975
+7.07295,0.67637,-1.16748,-1.53813,-1.06749,2.36571,-0.47746
+7.09964,0.67911,-1.16598,-1.53822,-1.06728,2.36349,-0.47517
+7.12633,0.68188,-1.16447,-1.53830,-1.06709,2.36126,-0.47287
+7.15302,0.68516,-1.16278,-1.53826,-1.06691,2.35860,-0.47015
+7.17972,0.68904,-1.16096,-1.53794,-1.06681,2.35545,-0.46695
+7.20641,0.69697,-1.15743,-1.53702,-1.06673,2.34900,-0.46049
+7.23310,0.70042,-1.15598,-1.53654,-1.06674,2.34620,-0.45771
+7.25979,0.70278,-1.15506,-1.53609,-1.06679,2.34427,-0.45581
+7.28648,0.70453,-1.15439,-1.53575,-1.06683,2.34284,-0.45441
+7.31317,0.70696,-1.15353,-1.53516,-1.06694,2.34086,-0.45247
+7.33986,0.70877,-1.15289,-1.53472,-1.06702,2.33938,-0.45103
+7.36655,0.71052,-1.15229,-1.53428,-1.06711,2.33795,-0.44964
+7.39324,0.71227,-1.15170,-1.53383,-1.06720,2.33651,-0.44825
+7.41993,0.71409,-1.15109,-1.53335,-1.06730,2.33502,-0.44682
+7.44662,0.71586,-1.15051,-1.53288,-1.06740,2.33358,-0.44543
+7.47331,0.71694,-1.15013,-1.53262,-1.06745,2.33268,-0.44457
+7.50000,0.71804,-1.14973,-1.53239,-1.06749,2.33178,-0.44371
+7.52669,0.71914,-1.14932,-1.53218,-1.06752,2.33088,-0.44285
+7.55338,0.71920,-1.14929,-1.53217,-1.06752,2.33083,-0.44280
+7.58007,0.72031,-1.14886,-1.53200,-1.06754,2.32992,-0.44194
+7.60676,0.72149,-1.14838,-1.53184,-1.06756,2.32895,-0.44102
+7.63345,0.72210,-1.14811,-1.53180,-1.06755,2.32845,-0.44054
+7.66014,0.72216,-1.14808,-1.53180,-1.06755,2.32840,-0.44050
+7.68683,0.72222,-1.14804,-1.53181,-1.06754,2.32835,-0.44045
+7.71352,0.72226,-1.14800,-1.53184,-1.06752,2.32832,-0.44042
+7.74021,0.72309,-1.14674,-1.53325,-1.06694,2.32764,-0.43977
+7.76690,0.72496,-1.14259,-1.53856,-1.06477,2.32608,-0.43829
+7.79359,0.72599,-1.13639,-1.54791,-1.06105,2.32521,-0.43744
+7.82028,0.72344,-1.12666,-1.56588,-1.05410,2.32727,-0.43933
+7.84698,0.71996,-1.12024,-1.57913,-1.04908,2.33010,-0.44198
+7.87367,0.71619,-1.11543,-1.58995,-1.04506,2.33318,-0.44488
+7.90036,0.71429,-1.11349,-1.59461,-1.04334,2.33473,-0.44635
+7.92705,0.71279,-1.11200,-1.59822,-1.04202,2.33595,-0.44752
+7.95374,0.71126,-1.11052,-1.60182,-1.04071,2.33720,-0.44870
+7.98043,0.70870,-1.10812,-1.60773,-1.03858,2.33928,-0.45070
+8.00712,0.70558,-1.10530,-1.61476,-1.03606,2.34182,-0.45316
+8.03381,0.70293,-1.10298,-1.62060,-1.03399,2.34397,-0.45525
+8.06050,0.70025,-1.10068,-1.62642,-1.03194,2.34615,-0.45737
+8.08719,0.69755,-1.09841,-1.63220,-1.02992,2.34834,-0.45953
+8.11388,0.69482,-1.09617,-1.63796,-1.02793,2.35056,-0.46171
+8.14057,0.69150,-1.09349,-1.64487,-1.02556,2.35325,-0.46439
+8.16726,0.68442,-1.08860,-1.65833,-1.02102,2.35897,-0.47014
+8.19395,0.67818,-1.08481,-1.66937,-1.01739,2.36400,-0.47526
+8.22064,0.67268,-1.08136,-1.67926,-1.01418,2.36843,-0.47983
+8.24733,0.66666,-1.07750,-1.69020,-1.01068,2.37326,-0.48487
+8.27402,0.66247,-1.07481,-1.69782,-1.00828,2.37661,-0.48841
+8.30071,0.65948,-1.07289,-1.70326,-1.00658,2.37901,-0.49095
+8.32740,0.65668,-1.07080,-1.70878,-1.00486,2.38124,-0.49334
+8.35409,0.65498,-1.06955,-1.71211,-1.00383,2.38260,-0.49480
+8.38078,0.65386,-1.06874,-1.71428,-1.00316,2.38349,-0.49576
+8.40747,0.65271,-1.06791,-1.71651,-1.00248,2.38441,-0.49675
+8.43416,0.65216,-1.06751,-1.71757,-1.00215,2.38485,-0.49722
+8.46085,0.65043,-1.06629,-1.72087,-1.00115,2.38622,-0.49871
+8.48754,0.64582,-1.06311,-1.72957,-0.99852,2.38988,-0.50272
+8.51423,0.64236,-1.06078,-1.73603,-0.99660,2.39263,-0.50575
+8.54093,0.63678,-1.05759,-1.74556,-0.99384,2.39704,-0.51066
+8.56762,0.63349,-1.05598,-1.75078,-0.99236,2.39965,-0.51359
+8.59431,0.63072,-1.05482,-1.75488,-0.99123,2.40183,-0.51606
+8.62100,0.62868,-1.05394,-1.75794,-0.99039,2.40344,-0.51789
+8.64769,0.62651,-1.05314,-1.76100,-0.98956,2.40514,-0.51985
+8.67438,0.62513,-1.05261,-1.76297,-0.98903,2.40623,-0.52110
+8.70107,0.62376,-1.05208,-1.76495,-0.98851,2.40730,-0.52233
+8.72776,0.62056,-1.05118,-1.76905,-0.98745,2.40982,-0.52525
+8.75445,0.61483,-1.05053,-1.77503,-0.98602,2.41431,-0.53051
+8.78114,0.60736,-1.05005,-1.78231,-0.98437,2.42014,-0.53745
+8.80783,0.60004,-1.05019,-1.78860,-0.98308,2.42583,-0.54435
+8.83452,0.59449,-1.05060,-1.79297,-0.98227,2.43013,-0.54963
+8.86121,0.59073,-1.05081,-1.79603,-0.98171,2.43304,-0.55325
+8.88790,0.58650,-1.05115,-1.79935,-0.98114,2.43630,-0.55734
+8.91459,0.58225,-1.05158,-1.80259,-0.98061,2.43957,-0.56149
+8.94128,0.57739,-1.05232,-1.80597,-0.98012,2.44330,-0.56627
+8.96797,0.57099,-1.05365,-1.80995,-0.97966,2.44819,-0.57263
+8.99466,0.56228,-1.05617,-1.81445,-0.97936,2.45481,-0.58141
+9.02135,0.55039,-1.06011,-1.82007,-0.97920,2.46379,-0.59362
+9.04804,0.54302,-1.06292,-1.82314,-0.97930,2.46932,-0.60134
+9.07473,0.53565,-1.06574,-1.82630,-0.97943,2.47483,-0.60917
+9.10142,0.53130,-1.06752,-1.82802,-0.97957,2.47807,-0.61383
+9.12811,0.52475,-1.07029,-1.83057,-0.97982,2.48291,-0.62093
+9.15480,0.51518,-1.07527,-1.83313,-0.98056,2.48995,-0.63146
+9.18149,0.48518,-1.09614,-1.83520,-0.98497,2.51166,-0.66578
+9.20819,0.47606,-1.10304,-1.83533,-0.98665,2.51815,-0.67663
+9.23488,0.46768,-1.10992,-1.83491,-0.98840,2.52407,-0.68677
+9.26157,0.46230,-1.11447,-1.83453,-0.98959,2.52784,-0.69337
+9.28826,0.46024,-1.11624,-1.83435,-0.99006,2.52928,-0.69591
+9.31495,0.45904,-1.11727,-1.83426,-0.99034,2.53011,-0.69740
+9.34164,0.45780,-1.11832,-1.83418,-0.99062,2.53098,-0.69894
+9.36833,0.45738,-1.11866,-1.83418,-0.99071,2.53127,-0.69947
+9.39502,0.44712,-1.12815,-1.83273,-0.99321,2.53837,-0.71237
+9.42171,0.44022,-1.13472,-1.83162,-0.99498,2.54309,-0.72119
+9.44840,0.43198,-1.14308,-1.82979,-0.99727,2.54870,-0.73190
+9.47509,0.42903,-1.14607,-1.82915,-0.99810,2.55068,-0.73576
+9.50178,0.42577,-1.14957,-1.82823,-0.99909,2.55288,-0.74008
+9.52847,0.42219,-1.15345,-1.82719,-1.00019,2.55527,-0.74484
+9.55516,0.41867,-1.15733,-1.82613,-1.00128,2.55763,-0.74957
+9.58185,0.41408,-1.16278,-1.82425,-1.00286,2.56067,-0.75576
+9.60854,0.40868,-1.16953,-1.82168,-1.00482,2.56423,-0.76313
+9.63523,0.40501,-1.17434,-1.81966,-1.00625,2.56664,-0.76820
+9.66192,0.40076,-1.18021,-1.81697,-1.00800,2.56941,-0.77410
+9.68861,0.39878,-1.18306,-1.81556,-1.00888,2.57070,-0.77687
+9.71530,0.39618,-1.18699,-1.81347,-1.01009,2.57238,-0.78052
+9.74199,0.39367,-1.19098,-1.81122,-1.01134,2.57400,-0.78408
+9.76868,0.39240,-1.19302,-1.81004,-1.01198,2.57481,-0.78587
+9.79537,0.39059,-1.19604,-1.80823,-1.01294,2.57598,-0.78846
+9.82206,0.38647,-1.20328,-1.80366,-1.01524,2.57862,-0.79435
+9.84875,0.38361,-1.20848,-1.80025,-1.01692,2.58044,-0.79848
+9.87544,0.38136,-1.21269,-1.79742,-1.01829,2.58186,-0.80175
+9.90214,0.37852,-1.21794,-1.79396,-1.01998,2.58365,-0.80587
+9.92883,0.37632,-1.22213,-1.79112,-1.02135,2.58504,-0.80910
+9.95552,0.37471,-1.22526,-1.78897,-1.02237,2.58605,-0.81147
+9.98221,0.37209,-1.23049,-1.78530,-1.02409,2.58769,-0.81534
+10.00890,0.37054,-1.23363,-1.78306,-1.02513,2.58866,-0.81764
+10.03559,0.36850,-1.23786,-1.78001,-1.02653,2.58993,-0.82069
+10.06228,0.36651,-1.24204,-1.77696,-1.02793,2.59116,-0.82366
+10.08897,0.36509,-1.24521,-1.77453,-1.02900,2.59204,-0.82579
+10.11566,0.36326,-1.24949,-1.77111,-1.03047,2.59316,-0.82855
+10.14235,0.36112,-1.25500,-1.76643,-1.03240,2.59448,-0.83179
+10.16904,0.35954,-1.25965,-1.76213,-1.03409,2.59545,-0.83420
+10.19573,0.35711,-1.26754,-1.75451,-1.03701,2.59694,-0.83793
+10.22242,0.35489,-1.27574,-1.74609,-1.04013,2.59830,-0.84135
+10.24911,0.35294,-1.28388,-1.73734,-1.04332,2.59949,-0.84438
+10.27580,0.35138,-1.29076,-1.72980,-1.04606,2.60043,-0.84681
+10.30249,0.35011,-1.29606,-1.72409,-1.04817,2.60121,-0.84881
+10.32918,0.34879,-1.30165,-1.71803,-1.05040,2.60200,-0.85087
+10.35587,0.34728,-1.30851,-1.71043,-1.05319,2.60292,-0.85326
+10.38256,0.34585,-1.31534,-1.70271,-1.05600,2.60378,-0.85552
+10.40925,0.34430,-1.32346,-1.69329,-1.05942,2.60471,-0.85799
+10.43594,0.34281,-1.33122,-1.68426,-1.06270,2.60560,-0.86036
+10.46263,0.34116,-1.34027,-1.67359,-1.06660,2.60659,-0.86301
+10.48932,0.33996,-1.34708,-1.66547,-1.06957,2.60731,-0.86494
+10.51601,0.33900,-1.35286,-1.65843,-1.07215,2.60788,-0.86649
+10.54270,0.33845,-1.35677,-1.65349,-1.07394,2.60820,-0.86738
+10.56940,0.33768,-1.36249,-1.64623,-1.07657,2.60866,-0.86864
+10.59609,0.33709,-1.36732,-1.63996,-1.07884,2.60902,-0.86962
+10.62278,0.33619,-1.37558,-1.62906,-1.08276,2.60956,-0.87111
+10.64947,0.33571,-1.38036,-1.62265,-1.08508,2.60985,-0.87191
+10.67616,0.33523,-1.38474,-1.61682,-1.08721,2.61013,-0.87271
+10.70285,0.33487,-1.38819,-1.61222,-1.08889,2.61035,-0.87332
+10.72954,0.33440,-1.39298,-1.60574,-1.09126,2.61063,-0.87410
+10.75623,0.33418,-1.39516,-1.60279,-1.09234,2.61076,-0.87447
+10.78292,0.33408,-1.39603,-1.60163,-1.09277,2.61082,-0.87464
+10.80961,0.33397,-1.39695,-1.60043,-1.09322,2.61089,-0.87482
+10.83630,0.33454,-1.41807,-1.56700,-1.10492,2.61059,-0.87413
+10.86299,0.33460,-1.41915,-1.56521,-1.10556,2.61056,-0.87404
+10.88968,0.33558,-1.43117,-1.54477,-1.11276,2.61001,-0.87263
+10.91637,0.33635,-1.43844,-1.53200,-1.11729,2.60957,-0.87150
+10.94306,0.33696,-1.44471,-1.52105,-1.12121,2.60923,-0.87061
+10.96975,0.33767,-1.45146,-1.50912,-1.12551,2.60883,-0.86958
+10.99644,0.33883,-1.46413,-1.48693,-1.13358,2.60817,-0.86789
+11.02313,0.33929,-1.46932,-1.47781,-1.13695,2.60791,-0.86724
+11.04982,0.33995,-1.47625,-1.46550,-1.14150,2.60753,-0.86628
+11.07651,0.34032,-1.48097,-1.45723,-1.14460,2.60732,-0.86575
+11.10320,0.34078,-1.48493,-1.45000,-1.14730,2.60706,-0.86509
+11.12989,0.34123,-1.48896,-1.44268,-1.15004,2.60680,-0.86444
+11.15658,0.34167,-1.49296,-1.43540,-1.15278,2.60655,-0.86380
+11.18327,0.34243,-1.49985,-1.42279,-1.15754,2.60611,-0.86269
+11.20996,0.34243,-1.49989,-1.42272,-1.15757,2.60611,-0.86269
+11.23665,0.34255,-1.50101,-1.42069,-1.15834,2.60604,-0.86252
+11.26335,0.34266,-1.50217,-1.41858,-1.15915,2.60598,-0.86236
+11.29004,0.34273,-1.50276,-1.41751,-1.15956,2.60594,-0.86227
+11.31673,0.34281,-1.50337,-1.41636,-1.15999,2.60590,-0.86215
+11.34342,0.34317,-1.50620,-1.41106,-1.16201,2.60568,-0.86162
+11.37011,0.34380,-1.51071,-1.40252,-1.16525,2.60532,-0.86071
+11.39680,0.34407,-1.51297,-1.39830,-1.16687,2.60517,-0.86031
+11.42349,0.34453,-1.51586,-1.39271,-1.16900,2.60490,-0.85964
+11.45018,0.34509,-1.51918,-1.38620,-1.17148,2.60457,-0.85881
+11.47687,0.34651,-1.52569,-1.37281,-1.17656,2.60374,-0.85671
+11.50356,0.34717,-1.52841,-1.36706,-1.17874,2.60335,-0.85572
+11.53025,0.34849,-1.53222,-1.35827,-1.18204,2.60258,-0.85375
+11.55694,0.34982,-1.53532,-1.35060,-1.18490,2.60179,-0.85175
+11.58363,0.35245,-1.53961,-1.33852,-1.18934,2.60022,-0.84781
+11.61032,0.35553,-1.54310,-1.32688,-1.19354,2.59837,-0.84318
+11.63701,0.36000,-1.54691,-1.31214,-1.19879,2.59567,-0.83651
+11.66370,0.36426,-1.55028,-1.29856,-1.20363,2.59308,-0.83020
+11.69039,0.36717,-1.55242,-1.28959,-1.20684,2.59130,-0.82593
+11.71708,0.36940,-1.55393,-1.28293,-1.20922,2.58992,-0.82266
+11.74377,0.37093,-1.55486,-1.27854,-1.21078,2.58898,-0.82043
+11.77046,0.37131,-1.55510,-1.27743,-1.21117,2.58874,-0.81987
+11.79715,0.37205,-1.55559,-1.27526,-1.21195,2.58829,-0.81879
+11.82384,0.37207,-1.55560,-1.27520,-1.21197,2.58827,-0.81877
+11.85053,0.37241,-1.55583,-1.27419,-1.21234,2.58806,-0.81827
+11.87722,0.37856,-1.56079,-1.25461,-1.21945,2.58424,-0.80940
+11.90391,0.38106,-1.56299,-1.24634,-1.22250,2.58267,-0.80583
+11.93060,0.38494,-1.56669,-1.23304,-1.22745,2.58023,-0.80031
+11.95730,0.38713,-1.56860,-1.22584,-1.23013,2.57885,-0.79722
+11.98399,0.39011,-1.57107,-1.21629,-1.23370,2.57696,-0.79304
+12.01068,0.39083,-1.57169,-1.21393,-1.23459,2.57650,-0.79203
+12.03737,0.39155,-1.57233,-1.21154,-1.23548,2.57604,-0.79102
+12.06406,0.39233,-1.57294,-1.20910,-1.23640,2.57554,-0.78993
+12.09075,0.39272,-1.57324,-1.20790,-1.23685,2.57530,-0.78939
+12.11744,0.39426,-1.57436,-1.20323,-1.23859,2.57431,-0.78724
+12.14413,0.39735,-1.57659,-1.19390,-1.24209,2.57232,-0.78295
+12.17082,0.40033,-1.57909,-1.18435,-1.24571,2.57041,-0.77886
+12.19751,0.40263,-1.58095,-1.17708,-1.24848,2.56892,-0.77570
+12.22420,0.40486,-1.58293,-1.16976,-1.25129,2.56748,-0.77267
+12.25089,0.40751,-1.58521,-1.16116,-1.25459,2.56575,-0.76907
+12.27758,0.40827,-1.58590,-1.15865,-1.25556,2.56526,-0.76804
+12.30427,0.40863,-1.58623,-1.15744,-1.25603,2.56502,-0.76756
+12.33096,0.40865,-1.58624,-1.15738,-1.25606,2.56501,-0.76753
+12.35765,0.40938,-1.58694,-1.15490,-1.25702,2.56453,-0.76654
+12.38434,0.41055,-1.58793,-1.15114,-1.25848,2.56377,-0.76497
+12.41103,0.41214,-1.58919,-1.14619,-1.26039,2.56273,-0.76284
+12.43772,0.41337,-1.59008,-1.14249,-1.26181,2.56192,-0.76118
+12.46441,0.41463,-1.59092,-1.13886,-1.26321,2.56109,-0.75951
+12.49110,0.41550,-1.59145,-1.13644,-1.26414,2.56052,-0.75834
+12.51779,0.41641,-1.59193,-1.13402,-1.26506,2.55992,-0.75713
+12.54448,0.41790,-1.59253,-1.13042,-1.26642,2.55893,-0.75514
+12.57117,0.41945,-1.59299,-1.12696,-1.26771,2.55791,-0.75308
+12.59786,0.42103,-1.59331,-1.12368,-1.26891,2.55685,-0.75098
+12.62456,0.42359,-1.59333,-1.11930,-1.27047,2.55515,-0.74760
+12.65125,0.42547,-1.59292,-1.11683,-1.27130,2.55390,-0.74512
+12.67794,0.42901,-1.59141,-1.11353,-1.27226,2.55153,-0.74047
+12.70463,0.43274,-1.58924,-1.11113,-1.27280,2.54901,-0.73559
+12.73132,0.43586,-1.58709,-1.10978,-1.27297,2.54691,-0.73154
+12.75801,0.43868,-1.58475,-1.10927,-1.27280,2.54499,-0.72789
+12.78470,0.44516,-1.57875,-1.10937,-1.27187,2.54057,-0.71956
+12.81139,0.45223,-1.57164,-1.11068,-1.27032,2.53570,-0.71058
+12.83808,0.46487,-1.55741,-1.11621,-1.26616,2.52691,-0.69482
+12.86477,0.46858,-1.55311,-1.11821,-1.26476,2.52431,-0.69026
+12.89146,0.47712,-1.54318,-1.12301,-1.26149,2.51828,-0.67989
+12.91815,0.48614,-1.53236,-1.12897,-1.25767,2.51186,-0.66911
+12.94484,0.49396,-1.52286,-1.13458,-1.25418,2.50625,-0.65991
+12.97153,0.49891,-1.51644,-1.13895,-1.25162,2.50267,-0.65415
+12.99822,0.50586,-1.50714,-1.14574,-1.24775,2.49763,-0.64615
+13.02491,0.51351,-1.49673,-1.15370,-1.24331,2.49204,-0.63747
+13.05160,0.51538,-1.49415,-1.15575,-1.24218,2.49067,-0.63537
+13.07829,0.51665,-1.49238,-1.15718,-1.24139,2.48973,-0.63394
+13.10498,0.51726,-1.49152,-1.15788,-1.24101,2.48929,-0.63326
+13.13167,0.52378,-1.48252,-1.16511,-1.23706,2.48448,-0.62600
+13.15836,0.54068,-1.45863,-1.18533,-1.22631,2.47191,-0.60761
+13.18505,0.54597,-1.45121,-1.19177,-1.22290,2.46794,-0.60196
+13.21174,0.55265,-1.44175,-1.20016,-1.21850,2.46291,-0.59491
+13.23843,0.55941,-1.43222,-1.20869,-1.21406,2.45779,-0.58787
+13.26512,0.56628,-1.42264,-1.21729,-1.20961,2.45257,-0.58080
+13.29181,0.57187,-1.41495,-1.22417,-1.20605,2.44830,-0.57510
+13.31851,0.57756,-1.40726,-1.23103,-1.20251,2.44394,-0.56938
+13.34520,0.58039,-1.40333,-1.23467,-1.20066,2.44177,-0.56655
+13.37189,0.58533,-1.39633,-1.24126,-1.19733,2.43796,-0.56164
+13.39858,0.59173,-1.38731,-1.24982,-1.19304,2.43301,-0.55535
+13.42527,0.59750,-1.37922,-1.25752,-1.18920,2.42853,-0.54974
+13.45196,0.60257,-1.37190,-1.26472,-1.18564,2.42459,-0.54485
+13.47865,0.60839,-1.36369,-1.27273,-1.18171,2.42004,-0.53930
+13.50534,0.61284,-1.35749,-1.27876,-1.17874,2.41656,-0.53509
+13.53203,0.61740,-1.35144,-1.28446,-1.17592,2.41298,-0.53082
+13.55872,0.62206,-1.34551,-1.28988,-1.17322,2.40931,-0.52649
+13.58541,0.62852,-1.33764,-1.29684,-1.16974,2.40421,-0.52054
+13.61210,0.63180,-1.33360,-1.30050,-1.16793,2.40162,-0.51755
+13.63879,0.63672,-1.32752,-1.30605,-1.16520,2.39771,-0.51309
+13.66548,0.64005,-1.32337,-1.30989,-1.16332,2.39507,-0.51009
+13.69217,0.64418,-1.31823,-1.31467,-1.16098,2.39178,-0.50639
+13.71886,0.64754,-1.31386,-1.31891,-1.15895,2.38910,-0.50341
+13.74555,0.65090,-1.30933,-1.32345,-1.15679,2.38642,-0.50043
+13.77224,0.65426,-1.30447,-1.32857,-1.15439,2.38373,-0.49747
+13.79893,0.65837,-1.29804,-1.33569,-1.15111,2.38043,-0.49388
+13.82562,0.66156,-1.29283,-1.34161,-1.14840,2.37787,-0.49109
+13.85231,0.66541,-1.28618,-1.34938,-1.14488,2.37476,-0.48775
+13.87900,0.66920,-1.27940,-1.35747,-1.14125,2.37171,-0.48448
+13.90569,0.67213,-1.27407,-1.36387,-1.13838,2.36934,-0.48196
+13.93238,0.67566,-1.26778,-1.37139,-1.13503,2.36648,-0.47895
+13.95907,0.68293,-1.25514,-1.38630,-1.12840,2.36058,-0.47281
+13.98577,0.68519,-1.25128,-1.39085,-1.12638,2.35875,-0.47091
+14.01246,0.68664,-1.24885,-1.39368,-1.12512,2.35757,-0.46970
+14.03915,0.68877,-1.24545,-1.39758,-1.12339,2.35584,-0.46793
+14.06584,0.69033,-1.24298,-1.40039,-1.12214,2.35457,-0.46663
+14.09253,0.69208,-1.24012,-1.40369,-1.12068,2.35314,-0.46518
+14.11922,0.69444,-1.23626,-1.40816,-1.11870,2.35121,-0.46322
+14.14591,0.69850,-1.22946,-1.41613,-1.11521,2.34790,-0.45989
+14.17260,0.70086,-1.22554,-1.42070,-1.11321,2.34597,-0.45796
+14.19929,0.70259,-1.22263,-1.42415,-1.11171,2.34456,-0.45655
+14.22598,0.70427,-1.21971,-1.42763,-1.11019,2.34318,-0.45518
+14.25267,0.70819,-1.21286,-1.43583,-1.10665,2.33996,-0.45200
+14.27936,0.71132,-1.20748,-1.44223,-1.10389,2.33740,-0.44949
+14.30605,0.71621,-1.19864,-1.45296,-1.09931,2.33337,-0.44557
+14.33274,0.71785,-1.19568,-1.45657,-1.09777,2.33202,-0.44426
+14.35943,0.71983,-1.19238,-1.46047,-1.09611,2.33039,-0.44270
+14.38612,0.72219,-1.18850,-1.46500,-1.09417,2.32845,-0.44083
+14.41281,0.72397,-1.18553,-1.46850,-1.09268,2.32697,-0.43942
+14.43950,0.72485,-1.18406,-1.47024,-1.09194,2.32625,-0.43873
+14.46619,0.72573,-1.18259,-1.47199,-1.09120,2.32553,-0.43804
+14.49288,0.72659,-1.18112,-1.47374,-1.09046,2.32481,-0.43736
+14.51957,0.72748,-1.17960,-1.47555,-1.08969,2.32407,-0.43666
+14.54626,0.72834,-1.17813,-1.47731,-1.08895,2.32337,-0.43599
+14.57295,0.72919,-1.17666,-1.47907,-1.08821,2.32266,-0.43533
+14.59964,0.73003,-1.17519,-1.48083,-1.08747,2.32196,-0.43467
+14.62633,0.73091,-1.17368,-1.48266,-1.08670,2.32124,-0.43399
+14.65302,0.73145,-1.17277,-1.48372,-1.08625,2.32079,-0.43356
+14.67972,0.73233,-1.17130,-1.48546,-1.08552,2.32007,-0.43288
+14.70641,0.73379,-1.16893,-1.48824,-1.08435,2.31885,-0.43174
+14.73310,0.73473,-1.16741,-1.49002,-1.08360,2.31808,-0.43102
+14.75979,0.73563,-1.16593,-1.49175,-1.08288,2.31733,-0.43032
+14.78648,0.73652,-1.16446,-1.49348,-1.08215,2.31659,-0.42963
+14.81317,0.73741,-1.16299,-1.49521,-1.08143,2.31585,-0.42894
+14.83986,0.73799,-1.16209,-1.49626,-1.08099,2.31538,-0.42850
+14.86655,0.73802,-1.16204,-1.49632,-1.08096,2.31535,-0.42848
+14.89324,0.73835,-1.16149,-1.49697,-1.08069,2.31508,-0.42823
+14.91993,0.73892,-1.16058,-1.49802,-1.08025,2.31461,-0.42779
+14.94662,0.74013,-1.15854,-1.50046,-1.07924,2.31360,-0.42685
+14.97331,0.74131,-1.15644,-1.50300,-1.07819,2.31262,-0.42595
+15.00000,0.74134,-1.15639,-1.50306,-1.07816,2.31260,-0.42593
diff --git a/python/examples/path_in_pixels.csv b/python/examples/path_in_pixels.csv
index e556fcf..2040e1f 100644
--- a/python/examples/path_in_pixels.csv
+++ b/python/examples/path_in_pixels.csv
@@ -1,58 +1,563 @@
-0.27487,0.52681
-0.27191,0.53469
-0.27191,0.53863
-0.27487,0.54651
-0.28226,0.55964
-0.29409,0.57409
-0.30592,0.58722
-0.31479,0.59641
-0.33105,0.60955
-0.36802,0.62924
-0.38428,0.63712
-0.40202,0.64500
-0.45081,0.66076
-0.46856,0.66470
-0.50256,0.66864
-0.52622,0.66733
-0.54100,0.66339
-0.55579,0.65945
-0.57353,0.65026
-0.58831,0.64369
-0.60014,0.63712
-0.60606,0.63318
-0.61641,0.62530
-0.62676,0.61086
-0.63415,0.59641
-0.63858,0.58722
-0.64154,0.57803
-0.64154,0.57015
-0.64154,0.54914
-0.63563,0.52287
-0.63267,0.50712
-0.62676,0.48479
-0.61641,0.45721
-0.60901,0.44277
-0.59866,0.43226
-0.57649,0.41650
-0.55874,0.40731
-0.52030,0.39287
-0.49813,0.39024
-0.47299,0.39155
-0.44194,0.39549
-0.41681,0.40468
-0.39463,0.41388
-0.35767,0.43095
-0.34880,0.43620
-0.33549,0.45327
-0.32810,0.46903
-0.32514,0.49004
-0.32366,0.52681
-0.32366,0.54389
-0.32514,0.56621
-0.32810,0.59379
-0.32958,0.60955
-0.33697,0.62924
-0.34584,0.64632
-0.36062,0.66339
-0.37097,0.66995
-0.37097,0.66995
+0.17153,0.75297
+0.18626,0.73858
+0.19952,0.72550
+0.21424,0.71111
+0.23191,0.69411
+0.24664,0.67710
+0.25695,0.66533
+0.26873,0.65487
+0.29671,0.63394
+0.30997,0.62086
+0.31880,0.61170
+0.33353,0.59993
+0.34384,0.58946
+0.35267,0.58162
+0.36298,0.57377
+0.37329,0.56200
+0.38066,0.55153
+0.38655,0.54368
+0.39096,0.53322
+0.39538,0.52799
+0.40569,0.51229
+0.41158,0.50052
+0.41747,0.49005
+0.42042,0.47959
+0.42336,0.47043
+0.42484,0.46128
+0.42778,0.45604
+0.43220,0.44689
+0.43809,0.43381
+0.44398,0.42334
+0.45134,0.40896
+0.45282,0.40242
+0.45723,0.39718
+0.46165,0.39064
+0.46460,0.38410
+0.46902,0.37887
+0.47638,0.36971
+0.48227,0.36187
+0.48963,0.35402
+0.49552,0.34748
+0.49994,0.34094
+0.50289,0.33832
+0.50583,0.33440
+0.50878,0.33047
+0.51320,0.32524
+0.51909,0.32132
+0.52350,0.31478
+0.53676,0.29385
+0.54412,0.27684
+0.55149,0.26115
+0.55443,0.25461
+0.55443,0.25199
+0.55443,0.24807
+0.55443,0.24676
+0.55443,0.24676
+0.55885,0.26246
+0.56327,0.27423
+0.57063,0.28862
+0.58241,0.31347
+0.60303,0.35140
+0.61187,0.36710
+0.62070,0.38803
+0.62659,0.40503
+0.63543,0.42465
+0.64574,0.44296
+0.65899,0.46258
+0.66635,0.47697
+0.67519,0.49136
+0.68403,0.50706
+0.69139,0.52275
+0.69728,0.53322
+0.70464,0.54630
+0.71054,0.55676
+0.71937,0.57115
+0.72821,0.58423
+0.73410,0.59600
+0.74146,0.60778
+0.74588,0.61824
+0.74735,0.62217
+0.74882,0.62478
+0.75030,0.62478
+0.75030,0.62609
+0.75472,0.63655
+0.76208,0.65225
+0.76502,0.66402
+0.76944,0.67449
+0.77386,0.68495
+0.77975,0.70326
+0.78417,0.71373
+0.78859,0.72419
+0.79153,0.73073
+0.79301,0.73858
+0.79448,0.74120
+0.79890,0.75035
+0.80184,0.75559
+0.80331,0.75951
+0.80479,0.76213
+0.80626,0.76474
+0.80626,0.76736
+0.80920,0.77259
+0.81215,0.77782
+0.81510,0.78567
+0.81804,0.79090
+0.81951,0.79221
+0.81951,0.79483
+0.81951,0.79613
+0.82099,0.79875
+0.82099,0.80006
+0.82099,0.80137
+0.80920,0.79613
+0.78859,0.78567
+0.77681,0.78044
+0.76355,0.77521
+0.75177,0.77128
+0.74146,0.76605
+0.73263,0.76082
+0.72232,0.75689
+0.71054,0.74904
+0.68697,0.73466
+0.67519,0.72942
+0.66783,0.72419
+0.65899,0.71765
+0.64721,0.71242
+0.63101,0.70457
+0.60008,0.69149
+0.58978,0.68495
+0.57358,0.67318
+0.55590,0.66271
+0.53676,0.65356
+0.52645,0.64833
+0.51467,0.64179
+0.50289,0.63655
+0.48374,0.62609
+0.47196,0.61955
+0.46018,0.61039
+0.44840,0.60124
+0.43809,0.59208
+0.42042,0.58031
+0.40569,0.57246
+0.39391,0.56461
+0.38507,0.55938
+0.37624,0.55284
+0.36446,0.54368
+0.35267,0.53583
+0.33942,0.52929
+0.32764,0.52275
+0.31291,0.51491
+0.30555,0.51098
+0.29524,0.50706
+0.28493,0.50313
+0.27315,0.49790
+0.26137,0.49136
+0.25106,0.48221
+0.23928,0.47174
+0.22750,0.45735
+0.22308,0.45212
+0.21866,0.44689
+0.21424,0.44427
+0.20982,0.44166
+0.20688,0.44035
+0.20393,0.43773
+0.20246,0.43642
+0.20099,0.43512
+0.19952,0.43381
+0.19657,0.43250
+0.19510,0.43119
+0.24959,0.43773
+0.25990,0.43773
+0.28199,0.44035
+0.30555,0.44296
+0.32322,0.44296
+0.33795,0.44296
+0.35415,0.44296
+0.37624,0.44558
+0.42484,0.44950
+0.45723,0.45343
+0.47343,0.45604
+0.48963,0.45735
+0.50141,0.45866
+0.52056,0.45997
+0.53970,0.46389
+0.55885,0.46520
+0.57063,0.46651
+0.59125,0.46782
+0.61039,0.46913
+0.62659,0.47043
+0.63837,0.47174
+0.66341,0.47567
+0.67814,0.47828
+0.68844,0.47959
+0.70023,0.48090
+0.70612,0.48221
+0.71348,0.48221
+0.71937,0.48351
+0.73263,0.48351
+0.74293,0.48351
+0.75619,0.48482
+0.76944,0.48613
+0.77533,0.48613
+0.78417,0.48613
+0.79006,0.48613
+0.79595,0.48744
+0.80184,0.48744
+0.80773,0.48744
+0.81657,0.48744
+0.82246,0.48744
+0.83129,0.48744
+0.83571,0.48744
+0.84160,0.48613
+0.84749,0.48482
+0.85044,0.48482
+0.85338,0.48482
+0.85486,0.48482
+0.85633,0.48482
+0.85486,0.48482
+0.85486,0.48482
+0.85338,0.48482
+0.85191,0.48482
+0.85044,0.48482
+0.84897,0.48482
+0.84897,0.48482
+0.84455,0.48744
+0.84160,0.48875
+0.83719,0.49136
+0.82393,0.49529
+0.81510,0.50052
+0.79742,0.50706
+0.75472,0.52275
+0.74293,0.52799
+0.72232,0.53583
+0.69875,0.54630
+0.68403,0.55153
+0.66488,0.56200
+0.60745,0.58423
+0.52498,0.60385
+0.50731,0.60778
+0.47785,0.61824
+0.44103,0.63132
+0.42336,0.63786
+0.39391,0.64833
+0.37329,0.65617
+0.35709,0.66533
+0.35120,0.67056
+0.34826,0.67318
+0.34678,0.67449
+0.32469,0.67972
+0.31438,0.68234
+0.29819,0.68757
+0.28788,0.69018
+0.28199,0.69280
+0.27462,0.69411
+0.27168,0.69542
+0.26726,0.69672
+0.26284,0.69934
+0.25695,0.70196
+0.25106,0.70457
+0.24517,0.70719
+0.23928,0.70980
+0.23339,0.71242
+0.22897,0.71373
+0.22602,0.71634
+0.22161,0.71765
+0.21719,0.71896
+0.21424,0.72027
+0.21277,0.72158
+0.20982,0.72288
+0.20688,0.72419
+0.20393,0.72550
+0.20099,0.72812
+0.19804,0.73204
+0.19068,0.73727
+0.18773,0.73989
+0.18626,0.74250
+0.18479,0.74381
+0.18332,0.74643
+0.18184,0.74774
+0.18037,0.74904
+0.17890,0.75035
+0.17743,0.75166
+0.17595,0.75297
+0.17448,0.75297
+0.17301,0.75297
+0.17153,0.75297
+0.17153,0.75297
+0.17006,0.75297
+0.16859,0.75297
+0.16712,0.75166
+0.16712,0.74904
+0.16712,0.74381
+0.16712,0.73073
+0.16712,0.71765
+0.16712,0.70850
+0.16859,0.69542
+0.17153,0.67449
+0.17301,0.66010
+0.17448,0.64833
+0.17595,0.64309
+0.17595,0.63917
+0.17595,0.63525
+0.17595,0.62871
+0.17595,0.62086
+0.17595,0.61432
+0.17595,0.60778
+0.17595,0.60124
+0.17595,0.59470
+0.17595,0.58685
+0.17743,0.57115
+0.17890,0.55807
+0.17890,0.54630
+0.17890,0.53322
+0.17890,0.52406
+0.17890,0.51752
+0.17743,0.51098
+0.17743,0.50706
+0.17743,0.50444
+0.17743,0.50183
+0.17743,0.50052
+0.17743,0.49659
+0.17743,0.48613
+0.17743,0.47828
+0.17890,0.46651
+0.18037,0.45997
+0.18184,0.45474
+0.18184,0.45081
+0.18332,0.44689
+0.18332,0.44427
+0.18332,0.44166
+0.18626,0.43642
+0.19215,0.42858
+0.19657,0.41811
+0.20246,0.40896
+0.20688,0.40242
+0.20835,0.39718
+0.21130,0.39195
+0.21424,0.38672
+0.21866,0.38149
+0.22455,0.37495
+0.23339,0.36710
+0.24370,0.35533
+0.25106,0.34879
+0.25695,0.34094
+0.26137,0.33701
+0.26726,0.33047
+0.27904,0.32393
+0.31733,0.30431
+0.32911,0.29777
+0.34089,0.29254
+0.34826,0.28862
+0.35120,0.28731
+0.35267,0.28600
+0.35415,0.28469
+0.35415,0.28338
+0.37035,0.27815
+0.38066,0.27292
+0.39391,0.26769
+0.39833,0.26507
+0.40422,0.26376
+0.41011,0.26115
+0.41600,0.25853
+0.42484,0.25722
+0.43514,0.25461
+0.44251,0.25330
+0.45134,0.25199
+0.45576,0.25199
+0.46165,0.25199
+0.46754,0.25199
+0.47049,0.25199
+0.47491,0.25199
+0.48522,0.25199
+0.49258,0.25199
+0.49847,0.25199
+0.50583,0.25068
+0.51172,0.25068
+0.51614,0.25068
+0.52350,0.25068
+0.52792,0.25068
+0.53381,0.25068
+0.53970,0.25068
+0.54412,0.25199
+0.55001,0.25330
+0.55738,0.25592
+0.56327,0.25984
+0.57358,0.26376
+0.58388,0.26900
+0.59419,0.27423
+0.60303,0.27815
+0.61039,0.27946
+0.61776,0.28208
+0.62659,0.28600
+0.63543,0.28992
+0.64574,0.29516
+0.65605,0.29908
+0.66783,0.30431
+0.67666,0.30824
+0.68403,0.31216
+0.68844,0.31608
+0.69581,0.32001
+0.70170,0.32393
+0.71201,0.33047
+0.71790,0.33440
+0.72379,0.33701
+0.72821,0.33963
+0.73410,0.34355
+0.73704,0.34486
+0.73852,0.34486
+0.73999,0.34486
+0.76061,0.37495
+0.76061,0.37756
+0.77386,0.39326
+0.78122,0.40372
+0.78859,0.41157
+0.79595,0.42073
+0.81068,0.43642
+0.81657,0.44296
+0.82393,0.45212
+0.82982,0.45735
+0.83277,0.46389
+0.83719,0.46913
+0.84160,0.47436
+0.84897,0.48351
+0.84897,0.48351
+0.85044,0.48482
+0.85191,0.48613
+0.85191,0.48744
+0.85191,0.48875
+0.85486,0.49267
+0.85928,0.49921
+0.86222,0.50183
+0.86369,0.50706
+0.86664,0.51229
+0.87106,0.52406
+0.87253,0.52929
+0.87253,0.53845
+0.87253,0.54630
+0.87106,0.55938
+0.86811,0.57246
+0.86517,0.58946
+0.86369,0.60516
+0.86222,0.61563
+0.86075,0.62347
+0.85928,0.62871
+0.85928,0.63001
+0.85928,0.63263
+0.85928,0.63263
+0.85928,0.63394
+0.85928,0.65617
+0.85928,0.66533
+0.85928,0.67972
+0.85780,0.68757
+0.85633,0.69803
+0.85633,0.70065
+0.85633,0.70326
+0.85486,0.70588
+0.85486,0.70719
+0.85338,0.71242
+0.85191,0.72288
+0.85191,0.73335
+0.85044,0.74120
+0.85044,0.74904
+0.84897,0.75820
+0.84897,0.76082
+0.84897,0.76213
+0.84897,0.76213
+0.84897,0.76474
+0.84749,0.76867
+0.84602,0.77390
+0.84455,0.77782
+0.84308,0.78175
+0.84160,0.78436
+0.84013,0.78698
+0.83719,0.79090
+0.83424,0.79483
+0.83129,0.79875
+0.82540,0.80398
+0.81951,0.80660
+0.81068,0.81183
+0.80184,0.81706
+0.79448,0.82099
+0.78711,0.82360
+0.77239,0.83145
+0.75619,0.83930
+0.72673,0.85107
+0.71790,0.85369
+0.69875,0.86154
+0.67814,0.86808
+0.66046,0.87331
+0.64868,0.87462
+0.63248,0.87723
+0.61481,0.87985
+0.61039,0.87985
+0.60745,0.87985
+0.60597,0.87985
+0.59125,0.88246
+0.55296,0.88508
+0.54118,0.88508
+0.52645,0.88508
+0.51172,0.88508
+0.49700,0.88508
+0.48522,0.88508
+0.47343,0.88508
+0.46754,0.88377
+0.45723,0.88246
+0.44398,0.88116
+0.43220,0.87985
+0.42189,0.87723
+0.41011,0.87592
+0.40127,0.87462
+0.39244,0.87462
+0.38360,0.87462
+0.37182,0.87462
+0.36593,0.87331
+0.35709,0.87200
+0.35120,0.87069
+0.34384,0.86938
+0.33795,0.86677
+0.33206,0.86415
+0.32617,0.86023
+0.31880,0.85500
+0.31291,0.85107
+0.30555,0.84584
+0.29819,0.84061
+0.29229,0.83668
+0.28493,0.83276
+0.27020,0.82491
+0.26579,0.82229
+0.26284,0.82099
+0.25842,0.81968
+0.25548,0.81837
+0.25253,0.81575
+0.24811,0.81314
+0.24075,0.80791
+0.23633,0.80529
+0.23339,0.80267
+0.23044,0.80006
+0.22308,0.79483
+0.21719,0.79090
+0.20835,0.78305
+0.20541,0.78044
+0.20099,0.77913
+0.19657,0.77651
+0.19363,0.77390
+0.19215,0.77259
+0.19068,0.77128
+0.18921,0.76997
+0.18773,0.76867
+0.18626,0.76736
+0.18479,0.76605
+0.18332,0.76474
+0.18184,0.76343
+0.18037,0.76343
+0.17890,0.76213
+0.17595,0.76082
+0.17448,0.75951
+0.17301,0.75820
+0.17153,0.75689
+0.17006,0.75559
+0.16859,0.75559
+0.16859,0.75559
+0.16859,0.75428
+0.16712,0.75428
+0.16564,0.75166
+0.16417,0.74904
+0.16417,0.74904
diff --git a/python/examples/plane_pose.pickle_save b/python/examples/plane_pose.pickle_save
index c1faa1600d3f78918b5c98fb85bce49a6db3c3dd..d664870c456e2d2ec0674aab04531818a00a3430 100644
GIT binary patch
delta 161
zcmX@ce3Y4`fo1Bhi7bBXqIspc1(kXeqk1eDgl%|lKT*ux&j1BeB(sl8&AqjM%4Dts
z{8Df2p~4JM{(h+ZWI0B8ah>CT7d=uvWq&YSLH|<s1$)aWsSKqHUQP~S)Ynn)&;4W{
s;2PAEnf(c5xWie$uloaBgLF=deg^R$>?-?j&#<_)!|3jr$?F-#0rIy!O#lD@
delta 162
zcmX@ge2kf;fo1CMi7b8`VtJ*x1(ka76C-;p8p@rc=a`D6>}P-iod?e(IzB(%zb;2t
z^E}H-d#G>&l)oP;KUtPhUR;9tZPfFuQ}zdnq%+&J@7TZKSFgAG_<V9OqrMJ9{`ar;
t0j@z)-nV@R8Q#EN{&|0ZYtS41Kc7H+g&S-B*fT6{?Qmi^FnJxLH~?0VKxO~{
diff --git a/python/ur_simple_control/args_to_robot.py b/python/ur_simple_control/args_to_robot.py
new file mode 100644
index 0000000..549ea62
--- /dev/null
+++ b/python/ur_simple_control/args_to_robot.py
@@ -0,0 +1,4 @@
+def getRobotFromArgs(args: argparse.Namespace):
+ if args.pinocchio_only:
+ pass
+ pass
diff --git a/python/ur_simple_control/networking/message_specs.proto b/python/ur_simple_control/networking/message_specs.proto
index 3048168..a0af620 100644
--- a/python/ur_simple_control/networking/message_specs.proto
+++ b/python/ur_simple_control/networking/message_specs.proto
@@ -8,3 +8,9 @@ message wrenches {
repeated double wrench = 1;
repeated double wrench_estimate = 2;
}
+
+message robot_target {
+ repeated double position = 1; // 3 double
+ repeated double rotation = 2; // 3 double
+ repeated double velocity = 2; // 6 double
+}
diff --git a/python/ur_simple_control/robots/heron.py b/python/ur_simple_control/robots/heron.py
new file mode 100644
index 0000000..5701db4
--- /dev/null
+++ b/python/ur_simple_control/robots/heron.py
@@ -0,0 +1,6 @@
+class RobotManagerHeron(RobotManagerAbstract):
+ def __init__(self, args):
+ self.args = args
+ self.model, self.collision_model, self.visual_model, self.data = (
+ heron_approximation()
+ )
diff --git a/python/ur_simple_control/robots/mobile_yumi.py b/python/ur_simple_control/robots/mobile_yumi.py
new file mode 100644
index 0000000..3e087f1
--- /dev/null
+++ b/python/ur_simple_control/robots/mobile_yumi.py
@@ -0,0 +1,6 @@
+
+class RobotManagerMobileYumi(RobotManagerAbstract):
+ def __init__(self, args):
+ self.args = args
+ self.model, self.collision_model, self.visual_model, self.data = (
+ get_yumi_model()
diff --git a/python/ur_simple_control/robots/robotmanager_abstract.py b/python/ur_simple_control/robots/robotmanager_abstract.py
index ff87510..a47a0e4 100644
--- a/python/ur_simple_control/robots/robotmanager_abstract.py
+++ b/python/ur_simple_control/robots/robotmanager_abstract.py
@@ -1,5 +1,8 @@
import abc
import argparse
+import pinocchio as pin
+
+from ur_simple_control.util.logging_utils import LogManager
###################################
# TODO: make everything different on each robot an abstract method.
@@ -7,7 +10,7 @@ import argparse
############################################
-class RobotManager(abc.ABC):
+class RobotManagerAbstract(abc.ABC):
"""
RobotManager:
---------------
@@ -37,48 +40,16 @@ class RobotManager(abc.ABC):
# might be changed later if that seems more appropriate
@abc.abstractmethod
def __init__(self, args):
- self.args: type
- self.pinocchio_only = args.pinocchio_only
- if self.args.simulation:
- self.args.robot_ip = "127.0.0.1"
- # load model
- # collision and visual models are none if args.visualize == False
- self.robot_name = args.robot
- if self.robot_name == "ur5e":
- self.model, self.collision_model, self.visual_model, self.data = get_model()
- if self.robot_name == "heron":
- self.model, self.collision_model, self.visual_model, self.data = (
- heron_approximation()
- )
- if self.robot_name == "heronros":
- self.model, self.collision_model, self.visual_model, self.data = (
- heron_approximation()
- )
- if self.robot_name == "mirros":
- self.model, self.collision_model, self.visual_model, self.data = (
- mir_approximation()
- )
- # self.publisher_vel_base = create_publisher(msg.Twist, '/cmd_vel', 5)
- # self.publisher_vel_base = publisher_vel_base
- if self.robot_name == "gripperlessur5e":
- self.model, self.collision_model, self.visual_model, self.data = (
- getGripperlessUR5e()
- )
- if self.robot_name == "yumi":
- self.model, self.collision_model, self.visual_model, self.data = (
- get_yumi_model()
- )
+ self.args: argparse.Namespace
+ self.robot_name: str = args.robot
+ self.model: pin.Model
+ self.data: pin.Data
+ self.visual_model: pin.pinocchio_pywrap_default.GeometryModel
+ self.collision_model: pin.pinocchio_pywrap_default.GeometryModel
- # create log manager if we're saving logs
if args.save_log:
self.log_manager = LogManager(args)
- # ur specific magic numbers
- # NOTE: all of this is ur-specific, and needs to be if-ed if other robots are added.
- # TODO: this is 8 in pinocchio and that's what you actually use
- # if we're being real lmao
- # the TODO here is make this consistent obviously
- self.n_arm_joints = 6
# last joint because pinocchio adds base frame as 0th joint.
# and since this is unintuitive, we add the other variable too
# so that the control designer doesn't need to think about such bs
diff --git a/python/ur_simple_control/robots/simulated_robot.py b/python/ur_simple_control/robots/simulated_robot.py
new file mode 100644
index 0000000..9adcb66
--- /dev/null
+++ b/python/ur_simple_control/robots/simulated_robot.py
@@ -0,0 +1,30 @@
+from ur_simple_control.robots.robotmanager_abstract import RobotManagerAbstract
+
+
+class RobotManagerSimulated(RobotManagerAbstract):
+ def __init__(self, args):
+ # TODO: idk where i pass args
+ super(RobotManagerAbstract, self).__init__(args)
+ if self.robot_name == "ur5e":
+ self.model, self.collision_model, self.visual_model, self.data = get_model()
+ if self.robot_name == "heron":
+ self.model, self.collision_model, self.visual_model, self.data = (
+ heron_approximation()
+ )
+ if self.robot_name == "heronros":
+ self.model, self.collision_model, self.visual_model, self.data = (
+ heron_approximation()
+ )
+ if self.robot_name == "mirros":
+ self.model, self.collision_model, self.visual_model, self.data = (
+ mir_approximation()
+ )
+ # self.publisher_vel_base = create_publisher(msg.Twist, '/cmd_vel', 5)
+ # self.publisher_vel_base = publisher_vel_base
+ if self.robot_name == "gripperlessur5e":
+ self.model, self.collision_model, self.visual_model, self.data = (
+ getGripperlessUR5e()
+ )
+ if self.robot_name == "yumi":
+ self.model, self.collision_model, self.visual_model, self.data = (
+ get_yumi_model()
diff --git a/python/ur_simple_control/robots/single_arm_interface.py b/python/ur_simple_control/robots/single_arm_interface.py
new file mode 100644
index 0000000..e69de29
diff --git a/python/ur_simple_control/robots/ur5e.py b/python/ur_simple_control/robots/ur5e.py
new file mode 100644
index 0000000..710db78
--- /dev/null
+++ b/python/ur_simple_control/robots/ur5e.py
@@ -0,0 +1,8 @@
+from ur_simple_control.robots.robotmanager_abstract import RobotManagerAbstract
+from ur_simple_control.util.get_model import get_model
+
+
+class RobotManagerUR5e(RobotManagerAbstract):
+ def __init__(self, args):
+ self.args = args
+ self.model, self.collision_model, self.visual_model, self.data = get_model()
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc
index 38b4a58b759b04ba21ce8753bda283693c29772c..bfffa078c75645aea48a88d67231247adeaa2e8c 100644
GIT binary patch
delta 2291
zcmey;&-i5mBj0jfUM>a(28LaiW~6@%+Q^s4YoyM=z);-F%)rpV@KjRng1py7N$)F?
z-q$5VE=h*m;4u2g%p}G3g@H+m?E;I~S2hMInaK-zE7<cc3j1CW_MNQ2=gM(_|Ds61
z6_J3+xqNQ*z88hOt_XQGcs~^po8obU!{Y)NePm`51sNmC*5Lb<jX_v+ipC8Np9^60
zk(o&tBqz+);0=-!o1#8fd5-#ZDcws_x+}6TN?Bczvf6=eFhVo3!A!!UlM^JhHPBUi
zU*G^C3};Nv=htS3DxSQMU(4eL2k!-b#ihnql#Q<_TkUYU;1v9knMnlXJ`uJCpRa5T
z;!;<{v{97V?kKt78upQyNgSk1oUOtCgTQ7^f%oie)u7O=n*2{Za#)7g=KGT8nb_8V
zJh@=<78!eQlwct{^uBU1@bGUol3l`SbR1;103vz1UX*mZBI$Nr(*KgA{|yddEXk8e
z-O2w3htLHPwYAJwG%T)Y*dGwN;2!ajnMo29J(6q}SVX_FG04bYkv6!&A#{U7>MjTO
z6%L6j97;Dhcy4e=e&lA8Vf(_sCc}1tMdGUfgP_Rd3F=Y0H#h{R8eb7szbLGEMObr%
z{dH~oOWO7qwH>c$J07t5$jk)N#01v#fot*`ORdR{8UdO&IAmtztaV=FeqG1rl8((q
z9lI+!b|0CUSV3-KW&6Ou#LD)S4P?g+7fs6>9KshwHCI?}$h&UldCAQ4qM7#<Gj9~L
zzj83BX)KAjpyvcZ7ge3Fs5(#P*Ho*&%fa2ld4ost29LrG9^M-~(${$uF7YT_<Wai9
zqjZ;tzsL6qkNih&HcqZD3~ZcSADKBouHXQ<f`gUqs~Cf{+#LNI9FpKrkpnY0L1waF
z=a9I>A#ssI>I#R{M_zVmwl56q(rg!5#2Z{YY(5Au@CaV#mcF!kyQUXAn;IhnL-EGR
z1<nqWXIQF}6l#2vXB$=x(GU~7!6A2pL*fPpDIqrbu50z?ZN?i|*;+xNra3vsGMK~=
zdt*t@5R;i?%>_$l!sI2h4<Zaave&uQFLA41<kq~xt=Zu6fsKJv>IR40bq?iA9Lg6t
zRIhNTVsLpraI*5SePCc^Vr84W(O7!(MC+UEY}Y~Ic4KmZb6z?$e=1)TR=pyux}g5L
zy7eV>>x=5PSJZ7U*av-NW|9ZxIeE4VEYcs?7!(Yz%UfQOx4gk256S<K<b8uf{5prs
zB@UU39CBAcQ7ZG1lU0H33j?bH+XWVx4?>gQSjtRJa?zh;B&vCXLt%l=R^~12*9{yl
z88}=taJpjPgg<lZuBhChciqbOl9lg8EB`B2{v>8@p&L9Z6lQL+lIrHWuF{O6-xwGe
zJ}@&fGJarUWn}rl03!G%^SVdX%LOtrntfnEA~Tq@7(Ez2Fd&ghj1nM4aKbExk<sb{
n0}@%sgv5bJ)G>K7GO~SOKq6C^!oV64q~Hez1gFS{fq?-4JJHwo
delta 2191
zcmew|f$>W}Bj0jfUM>a(28QF8<J0T?H}WO&8mTZaFcfDqGcYtTJe8EYAn$ci())^}
z_jSpTOOhcMLhC;=Fi1HveF2jfSj6s1N>5(MTfrW8QP}5-u+L-#K39$%^%q6_uZZ|h
z&gFBfx4kIjc}2*x!TYI**c6Wovat|!Q6%n)NL+*OT_KSv8W*JFA?Tt|!WE%}2JgE<
zqEpo8D$h~BE~R@(N_R!}MJcN*QdT?A4NJTr3qlt~lCFp(!431gAPqtng}kl^O?Kgz
zmPOZ=a6uM?E{Y^x5lNif%xAz3RWW%dzn0w!(F+DnN1d;@gj{inz7U&uA-UwDaOoA{
z(gvTqVv<+Hv{$%ZFm^p!e8oNdihJUP<n#-fRTssouZUGQ_<!KptS#`Koh=g-ib<0d
zBqE1N$ZVFBI?u#53FNIQli$hM+fourPba?;kls8|wwqIDCCJ3TOz`C9dQsBtilp0h
zN&icd{uctGFG|K-k&J;RG(UAGpA}gbtbEQUU-65(;+J|MJ^Mm#-9?G|D-!h=SVZqi
z%UzK+*dTSm*8gf?$d$mvD}mV;atl8&FqAO5GJRxVkZ}X0Tp2f}3oH^(1%xNxSC7)&
zka<-2ieu<S$M7qT;Wv1suk$Ee;!(KBqjZHwX@S>AW=0u<FAR(_1{YW)K5$I-x6+zC
zK_fu%f`-kGoU_hn+^_q@T=I#z=o5FvC+;Fk{1uk?3oP+>1%xMaYHFIS$i86he<3jH
zLQK~6nEXpI`4?jfuf!BylrFj=T?BH2s``?M3wjO^bWzpuimKz}Y)!TLxy5rTSIAy4
z_delxBLBKy(j~v7i+(9r{8Fx_W?V@vx{_LPQNHqueC0)!>MJbO7g(ylvN1@@&C$Q0
zWxXTnf@l1N#G(r&rB}<Vu9Vk(XJDvc^kDh~p+15rFHkU4FnWQ)MA{p~k@jY~$Rgh0
z+F^5pTj2U;UM*jCwpXCISw1<zFrB18;+g!#uxc;|P~-)VxC;pd7mA9nmR4LTt@+Nt
zP{!y63Lp^m5ybNVg-IEs2RMK{K^$pMlmMD-tjES?%E-V_%s%<NWiW{W<Znge0Fs$p
zV9k{SPDSDu3|$W-p5VXk5p&5S=AuX36_2>9@kv+WbFRb}U6e1rB42!wrSu9*=>?Y3
z2i!8(xz#Umt6${Syuz*7;PQZ5=7OBbbvesRa+Vk6tgpyfU$70nZX0pQHsYdf)D_#P
z3o!{7xDzh0But(tE4^9F<|aE^2`E$^OinO#m~85zm$pOlE)V||9+`_ga#wid&@$Ks
z74wg*O!Agr7?|WOFR(~|U}I1)ye@BfN#61Xk1}4379ZJ}6|BB6Fe_MHV3C>ZYb7`N
znTx*r4Icgp!XKF#MZ~@^Fp7vZ_<UevU=_R0qJD`*{ep(W4Goj6$y?H|o4H*wbGvBf
zamCEzBQqnb7vmQO5cvU2f{F%M_6@!e8h0Tf?|MS<rG(;(38hyON-s*6U6C#$n0>`A
z7<ilzJYj#`JN}Y){6+7?E8dA$lTxoF<zGoEy(nLHMZWAJOT`tIiVG|iNZFSPsdMsd
z<GRUL-4=*`W?*3Wz|6?V_<@O)k>vveh~Syr<sMZp<;=)v_JIM3^kPzC^kDqJfJ6o{
xihvZs2|YVTs}Brt!imX)5hQ>_hB28lCNh3tKqBiHox!S5NItkwkq-j{0{~p`LQ4Pu
diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py
index b691c5e..351790f 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -4,6 +4,7 @@ possible improvement:
and then put it here.
these magic numbers are not a good look.
"""
+
import pinocchio as pin
import numpy as np
import sys
@@ -33,46 +34,62 @@ having that said, supposedly there is a big missalignment (few cm)
between the actual robot and the non-calibrated model.
NOTE: this should be fixed for a proper release
"""
+
+
def get_model():
-
- urdf_path_relative = files('ur_simple_control.robot_descriptions.urdf').joinpath('ur5e_with_robotiq_hande_FIXED_PATHS.urdf')
+
+ urdf_path_relative = files("ur_simple_control.robot_descriptions.urdf").joinpath(
+ "ur5e_with_robotiq_hande_FIXED_PATHS.urdf"
+ )
urdf_path_absolute = os.path.abspath(urdf_path_relative)
- mesh_dir = files('ur_simple_control')
+ mesh_dir = files("ur_simple_control")
mesh_dir_absolute = os.path.abspath(mesh_dir)
shoulder_trans = np.array([0, 0, 0.1625134425523304])
shoulder_rpy = np.array([-0, 0, 5.315711138647629e-08])
- shoulder_se3 = pin.SE3(pin.rpy.rpyToMatrix(shoulder_rpy),shoulder_trans)
+ shoulder_se3 = pin.SE3(pin.rpy.rpyToMatrix(shoulder_rpy), shoulder_trans)
upper_arm_trans = np.array([0.000300915150907851, 0, 0])
upper_arm_rpy = np.array([1.571659987714477, 0, 1.155342090832558e-06])
- upper_arm_se3 = pin.SE3(pin.rpy.rpyToMatrix(upper_arm_rpy),upper_arm_trans)
+ upper_arm_se3 = pin.SE3(pin.rpy.rpyToMatrix(upper_arm_rpy), upper_arm_trans)
forearm_trans = np.array([-0.4249536100418752, 0, 0])
forearm_rpy = np.array([3.140858652067472, 3.141065383898231, 3.141581851193229])
- forearm_se3 = pin.SE3(pin.rpy.rpyToMatrix(forearm_rpy),forearm_trans)
-
- wrist_1_trans = np.array([-0.3922353894477613, -0.001171506236920081, 0.1337997346972175])
- wrist_1_rpy = np.array([0.008755445624588536, 0.0002860523431017214, 7.215921353974553e-06])
- wrist_1_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_1_rpy),wrist_1_trans)
-
- wrist_2_trans = np.array([5.620166987673597e-05, -0.09948910981796041, 0.0002201494606859632])
+ forearm_se3 = pin.SE3(pin.rpy.rpyToMatrix(forearm_rpy), forearm_trans)
+
+ wrist_1_trans = np.array(
+ [-0.3922353894477613, -0.001171506236920081, 0.1337997346972175]
+ )
+ wrist_1_rpy = np.array(
+ [0.008755445624588536, 0.0002860523431017214, 7.215921353974553e-06]
+ )
+ wrist_1_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_1_rpy), wrist_1_trans)
+
+ wrist_2_trans = np.array(
+ [5.620166987673597e-05, -0.09948910981796041, 0.0002201494606859632]
+ )
wrist_2_rpy = np.array([1.568583530823855, 0, -3.513049549874747e-07])
- wrist_2_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_2_rpy),wrist_2_trans)
+ wrist_2_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_2_rpy), wrist_2_trans)
- wrist_3_trans = np.array([9.062061300900664e-06, 0.09947787349620175, 0.0001411778743239612])
+ wrist_3_trans = np.array(
+ [9.062061300900664e-06, 0.09947787349620175, 0.0001411778743239612]
+ )
wrist_3_rpy = np.array([1.572215514545703, 3.141592653589793, 3.141592633687631])
- wrist_3_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_3_rpy),wrist_3_trans)
+ wrist_3_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_3_rpy), wrist_3_trans)
model = None
collision_model = None
visual_model = None
# this command just calls the ones below it. both are kept here
# in case pinocchio people decide to change their api.
- #model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path_absolute, mesh_dir_absolute)
+ # model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path_absolute, mesh_dir_absolute)
model = pin.buildModelFromUrdf(urdf_path_absolute)
- 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)
+ 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.
@@ -97,44 +114,53 @@ def get_model():
model.jointPlacements[3] = forearm_se3
model.jointPlacements[4] = wrist_1_se3
model.jointPlacements[5] = wrist_2_se3
- model.jointPlacements[6] = wrist_3_se3
+ model.jointPlacements[6] = wrist_3_se3
data = pin.Data(model)
return model, collision_model, visual_model, data
+
def getGripperlessUR5e():
robot = example_robot_data.load("ur5")
shoulder_trans = np.array([0, 0, 0.1625134425523304])
shoulder_rpy = np.array([-0, 0, 5.315711138647629e-08])
- shoulder_se3 = pin.SE3(pin.rpy.rpyToMatrix(shoulder_rpy),shoulder_trans)
+ shoulder_se3 = pin.SE3(pin.rpy.rpyToMatrix(shoulder_rpy), shoulder_trans)
upper_arm_trans = np.array([0.000300915150907851, 0, 0])
upper_arm_rpy = np.array([1.571659987714477, 0, 1.155342090832558e-06])
- upper_arm_se3 = pin.SE3(pin.rpy.rpyToMatrix(upper_arm_rpy),upper_arm_trans)
+ upper_arm_se3 = pin.SE3(pin.rpy.rpyToMatrix(upper_arm_rpy), upper_arm_trans)
forearm_trans = np.array([-0.4249536100418752, 0, 0])
forearm_rpy = np.array([3.140858652067472, 3.141065383898231, 3.141581851193229])
- forearm_se3 = pin.SE3(pin.rpy.rpyToMatrix(forearm_rpy),forearm_trans)
-
- wrist_1_trans = np.array([-0.3922353894477613, -0.001171506236920081, 0.1337997346972175])
- wrist_1_rpy = np.array([0.008755445624588536, 0.0002860523431017214, 7.215921353974553e-06])
- wrist_1_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_1_rpy),wrist_1_trans)
-
- wrist_2_trans = np.array([5.620166987673597e-05, -0.09948910981796041, 0.0002201494606859632])
+ forearm_se3 = pin.SE3(pin.rpy.rpyToMatrix(forearm_rpy), forearm_trans)
+
+ wrist_1_trans = np.array(
+ [-0.3922353894477613, -0.001171506236920081, 0.1337997346972175]
+ )
+ wrist_1_rpy = np.array(
+ [0.008755445624588536, 0.0002860523431017214, 7.215921353974553e-06]
+ )
+ wrist_1_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_1_rpy), wrist_1_trans)
+
+ wrist_2_trans = np.array(
+ [5.620166987673597e-05, -0.09948910981796041, 0.0002201494606859632]
+ )
wrist_2_rpy = np.array([1.568583530823855, 0, -3.513049549874747e-07])
- wrist_2_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_2_rpy),wrist_2_trans)
+ wrist_2_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_2_rpy), wrist_2_trans)
- wrist_3_trans = np.array([9.062061300900664e-06, 0.09947787349620175, 0.0001411778743239612])
+ wrist_3_trans = np.array(
+ [9.062061300900664e-06, 0.09947787349620175, 0.0001411778743239612]
+ )
wrist_3_rpy = np.array([1.572215514545703, 3.141592653589793, 3.141592633687631])
- wrist_3_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_3_rpy),wrist_3_trans)
+ wrist_3_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_3_rpy), wrist_3_trans)
robot.model.jointPlacements[1] = shoulder_se3
robot.model.jointPlacements[2] = upper_arm_se3
robot.model.jointPlacements[3] = forearm_se3
robot.model.jointPlacements[4] = wrist_1_se3
robot.model.jointPlacements[5] = wrist_2_se3
- robot.model.jointPlacements[6] = wrist_3_se3
+ robot.model.jointPlacements[6] = wrist_3_se3
data = pin.Data(robot.model)
return robot.model, robot.collision_model, robot.visual_model, data
@@ -144,11 +170,11 @@ def getGripperlessUR5e():
# TODO: look what's done in pink, see if this can be usable
# after you've removed camera joint and similar.
def get_heron_model():
-
- #urdf_path_relative = files('ur_simple_control.robot_descriptions.urdf').joinpath('ur5e_with_robotiq_hande_FIXED_PATHS.urdf')
+
+ # urdf_path_relative = files('ur_simple_control.robot_descriptions.urdf').joinpath('ur5e_with_robotiq_hande_FIXED_PATHS.urdf')
urdf_path_absolute = "/home/gospodar/home2/gospodar/lund/praxis/software/ros/ros-containers/home/model.urdf"
- #mesh_dir = files('ur_simple_control')
- #mesh_dir_absolute = os.path.abspath(mesh_dir)
+ # mesh_dir = files('ur_simple_control')
+ # mesh_dir_absolute = os.path.abspath(mesh_dir)
mesh_dir_absolute = "/home/gospodar/lund/praxis/software/ros/ros-containers/home/heron_description/MIR_robot"
model = None
@@ -156,10 +182,14 @@ def get_heron_model():
visual_model = None
# this command just calls the ones below it. both are kept here
# in case pinocchio people decide to change their api.
- #model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path_absolute, mesh_dir_absolute)
+ # model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path_absolute, mesh_dir_absolute)
model = pin.buildModelFromUrdf(urdf_path_absolute)
- 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)
+ 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
+ )
data = pin.Data(model)
@@ -167,12 +197,14 @@ def get_heron_model():
def get_yumi_model():
-
- urdf_path_relative = files('ur_simple_control.robot_descriptions').joinpath('yumi.urdf')
+
+ urdf_path_relative = files("ur_simple_control.robot_descriptions").joinpath(
+ "yumi.urdf"
+ )
urdf_path_absolute = os.path.abspath(urdf_path_relative)
- #mesh_dir = files('ur_simple_control')
- #mesh_dir_absolute = os.path.abspath(mesh_dir)
- #mesh_dir_absolute = "/home/gospodar/lund/praxis/software/ros/ros-containers/home/heron_description/MIR_robot"
+ # mesh_dir = files('ur_simple_control')
+ # mesh_dir_absolute = os.path.abspath(mesh_dir)
+ # mesh_dir_absolute = "/home/gospodar/lund/praxis/software/ros/ros-containers/home/heron_description/MIR_robot"
mesh_dir_absolute = None
model = None
@@ -180,10 +212,18 @@ def get_yumi_model():
visual_model = None
# this command just calls the ones below it. both are kept here
# in case pinocchio people decide to change their api.
- #model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path_absolute, mesh_dir_absolute)
+ # model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path_absolute, mesh_dir_absolute)
model_arms = pin.buildModelFromUrdf(urdf_path_absolute)
- visual_model_arms = pin.buildGeomFromUrdf(model_arms, urdf_path_absolute, pin.GeometryType.VISUAL, None, mesh_dir_absolute)
- collision_model_arms = pin.buildGeomFromUrdf(model_arms, urdf_path_absolute, pin.GeometryType.COLLISION, None, mesh_dir_absolute)
+ visual_model_arms = pin.buildGeomFromUrdf(
+ model_arms, urdf_path_absolute, pin.GeometryType.VISUAL, None, mesh_dir_absolute
+ )
+ collision_model_arms = pin.buildGeomFromUrdf(
+ model_arms,
+ urdf_path_absolute,
+ pin.GeometryType.COLLISION,
+ None,
+ mesh_dir_absolute,
+ )
data_arms = pin.Data(model_arms)
@@ -196,10 +236,11 @@ def get_yumi_model():
parent_id = 0
# TEST
joint_placement = pin.SE3.Identity()
- #joint_placement.rotation = pin.rpy.rpyToMatrix(0, -np.pi/2, 0)
- #joint_placement.translation[2] = 0.2
- MOBILE_BASE_JOINT_ID = model_mobile_base.addJoint(parent_id, pin.JointModelPlanar(),
- joint_placement.copy(), joint_name)
+ # joint_placement.rotation = pin.rpy.rpyToMatrix(0, -np.pi/2, 0)
+ # joint_placement.translation[2] = 0.2
+ MOBILE_BASE_JOINT_ID = model_mobile_base.addJoint(
+ parent_id, pin.JointModelPlanar(), joint_placement.copy(), joint_name
+ )
# we should immediately set velocity limits.
# there are no position limit by default and that is what we want.
model_mobile_base.velocityLimit[0] = 2
@@ -213,26 +254,45 @@ def get_yumi_model():
# TODO: find heron (mir) numbers
body_inertia = pin.Inertia.FromBox(30, 0.5, 0.3, 0.4)
# maybe change placement to sth else depending on where its grasped
- model_mobile_base.appendBodyToJoint(MOBILE_BASE_JOINT_ID, body_inertia, pin.SE3.Identity())
- box_shape = fcl.Box(0.5, 0.3, 0.4)
+ model_mobile_base.appendBodyToJoint(
+ MOBILE_BASE_JOINT_ID, body_inertia, pin.SE3.Identity()
+ )
+ box_shape = fcl.Box(0.5, 0.3, 0.4)
body_placement = pin.SE3.Identity()
- geometry_mobile_base = pin.GeometryObject("box_shape", MOBILE_BASE_JOINT_ID, box_shape, body_placement.copy())
+ geometry_mobile_base = pin.GeometryObject(
+ "box_shape", MOBILE_BASE_JOINT_ID, box_shape, body_placement.copy()
+ )
- geometry_mobile_base.meshColor = np.array([1., 0.1, 0.1, 1.])
+ geometry_mobile_base.meshColor = np.array([1.0, 0.1, 0.1, 1.0])
geom_model_mobile_base.addGeometryObject(geometry_mobile_base)
# have to add the frame manually
- model_mobile_base.addFrame(pin.Frame('mobile_base',MOBILE_BASE_JOINT_ID,0,joint_placement.copy(),pin.FrameType.JOINT) )
+ model_mobile_base.addFrame(
+ pin.Frame(
+ "mobile_base",
+ MOBILE_BASE_JOINT_ID,
+ 0,
+ joint_placement.copy(),
+ pin.FrameType.JOINT,
+ )
+ )
# frame-index should be 1
- model, visual_model = pin.appendModel(model_mobile_base, model_arms, geom_model_mobile_base, visual_model_arms, 1, pin.SE3.Identity())
+ model, visual_model = pin.appendModel(
+ model_mobile_base,
+ model_arms,
+ geom_model_mobile_base,
+ visual_model_arms,
+ 1,
+ pin.SE3.Identity(),
+ )
data = model.createData()
return model, visual_model.copy(), visual_model, data
def heron_approximation():
- # arm + gripper
+ # arm + gripper
model_arm, collision_model_arm, visual_model_arm, data_arm = get_model()
# mobile base as planar joint (there's probably a better
@@ -244,8 +304,8 @@ def heron_approximation():
parent_id = 0
# TEST
joint_placement = pin.SE3.Identity()
- #joint_placement.rotation = pin.rpy.rpyToMatrix(0, -np.pi/2, 0)
- #joint_placement.translation[2] = 0.2
+ # joint_placement.rotation = pin.rpy.rpyToMatrix(0, -np.pi/2, 0)
+ # joint_placement.translation[2] = 0.2
# TODO TODO TODO TODO TODO TODO TODO TODO
# TODO: heron is actually a differential drive,
# meaning that it is not a planar joint.
@@ -254,8 +314,9 @@ def heron_approximation():
# and that should work for our purposes.
# this makes sense for initial testing
# because mobile yumi's base is a planar joint
- MOBILE_BASE_JOINT_ID = model_mobile_base.addJoint(parent_id, pin.JointModelPlanar(),
- joint_placement.copy(), joint_name)
+ MOBILE_BASE_JOINT_ID = model_mobile_base.addJoint(
+ parent_id, pin.JointModelPlanar(), joint_placement.copy(), joint_name
+ )
# we should immediately set velocity limits.
# there are no position limit by default and that is what we want.
# TODO: put in heron's values
@@ -263,37 +324,56 @@ def heron_approximation():
model_mobile_base.velocityLimit[0] = 2
# TODO: PUT THE CONSTRAINTS BACK!!!!!!!!!!!!!!!
model_mobile_base.velocityLimit[1] = 0
- #model_mobile_base.velocityLimit[1] = 2
+ # model_mobile_base.velocityLimit[1] = 2
model_mobile_base.velocityLimit[2] = 2
# TODO: i have literally no idea what reasonable numbers are here
model_mobile_base.effortLimit[0] = 200
# TODO: PUT THE CONSTRAINTS BACK!!!!!!!!!!!!!!!
model_mobile_base.effortLimit[1] = 0
- #model_mobile_base.effortLimit[1] = 2
+ # model_mobile_base.effortLimit[1] = 2
model_mobile_base.effortLimit[2] = 200
- #print("OBJECT_JOINT_ID",OBJECT_JOINT_ID)
- #body_inertia = pin.Inertia.FromBox(args.box_mass, box_dimensions[0],
+ # print("OBJECT_JOINT_ID",OBJECT_JOINT_ID)
+ # body_inertia = pin.Inertia.FromBox(args.box_mass, box_dimensions[0],
# box_dimensions[1], box_dimensions[2])
# pretty much random numbers
# TODO: find heron (mir) numbers
body_inertia = pin.Inertia.FromBox(30, 0.5, 0.3, 0.4)
# maybe change placement to sth else depending on where its grasped
- model_mobile_base.appendBodyToJoint(MOBILE_BASE_JOINT_ID, body_inertia, pin.SE3.Identity())
- box_shape = fcl.Box(0.5, 0.3, 0.4)
+ model_mobile_base.appendBodyToJoint(
+ MOBILE_BASE_JOINT_ID, body_inertia, pin.SE3.Identity()
+ )
+ box_shape = fcl.Box(0.5, 0.3, 0.4)
body_placement = pin.SE3.Identity()
- geometry_mobile_base = pin.GeometryObject("box_shape", MOBILE_BASE_JOINT_ID, box_shape, body_placement.copy())
+ geometry_mobile_base = pin.GeometryObject(
+ "box_shape", MOBILE_BASE_JOINT_ID, box_shape, body_placement.copy()
+ )
- geometry_mobile_base.meshColor = np.array([1., 0.1, 0.1, 1.])
+ geometry_mobile_base.meshColor = np.array([1.0, 0.1, 0.1, 1.0])
geom_model_mobile_base.addGeometryObject(geometry_mobile_base)
# have to add the frame manually
- model_mobile_base.addFrame(pin.Frame('mobile_base',MOBILE_BASE_JOINT_ID,0,joint_placement.copy(),pin.FrameType.JOINT) )
+ model_mobile_base.addFrame(
+ pin.Frame(
+ "mobile_base",
+ MOBILE_BASE_JOINT_ID,
+ 0,
+ joint_placement.copy(),
+ pin.FrameType.JOINT,
+ )
+ )
# frame-index should be 1
- model, visual_model = pin.appendModel(model_mobile_base, model_arm, geom_model_mobile_base, visual_model_arm, 1, pin.SE3.Identity())
+ model, visual_model = pin.appendModel(
+ model_mobile_base,
+ model_arm,
+ geom_model_mobile_base,
+ visual_model_arm,
+ 1,
+ pin.SE3.Identity(),
+ )
data = model.createData()
-
+
# fix gripper
for geom in visual_model.geometryObjects:
if "hand" in geom.name:
@@ -316,8 +396,8 @@ def mir_approximation():
parent_id = 0
# TEST
joint_placement = pin.SE3.Identity()
- #joint_placement.rotation = pin.rpy.rpyToMatrix(0, -np.pi/2, 0)
- #joint_placement.translation[2] = 0.2
+ # joint_placement.rotation = pin.rpy.rpyToMatrix(0, -np.pi/2, 0)
+ # joint_placement.translation[2] = 0.2
# TODO TODO TODO TODO TODO TODO TODO TODO
# TODO: heron is actually a differential drive,
# meaning that it is not a planar joint.
@@ -326,8 +406,9 @@ def mir_approximation():
# and that should work for our purposes.
# this makes sense for initial testing
# because mobile yumi's base is a planar joint
- MOBILE_BASE_JOINT_ID = model_mobile_base.addJoint(parent_id, pin.JointModelPlanar(),
- joint_placement.copy(), joint_name)
+ MOBILE_BASE_JOINT_ID = model_mobile_base.addJoint(
+ parent_id, pin.JointModelPlanar(), joint_placement.copy(), joint_name
+ )
# we should immediately set velocity limits.
# there are no position limit by default and that is what we want.
# TODO: put in heron's values
@@ -339,38 +420,56 @@ def mir_approximation():
model_mobile_base.effortLimit[0] = 200
model_mobile_base.effortLimit[1] = 0
model_mobile_base.effortLimit[2] = 200
- #print("OBJECT_JOINT_ID",OBJECT_JOINT_ID)
- #body_inertia = pin.Inertia.FromBox(args.box_mass, box_dimensions[0],
+ # print("OBJECT_JOINT_ID",OBJECT_JOINT_ID)
+ # body_inertia = pin.Inertia.FromBox(args.box_mass, box_dimensions[0],
# box_dimensions[1], box_dimensions[2])
# pretty much random numbers
# TODO: find heron (mir) numbers
body_inertia = pin.Inertia.FromBox(30, 0.5, 0.3, 0.4)
# maybe change placement to sth else depending on where its grasped
- model_mobile_base.appendBodyToJoint(MOBILE_BASE_JOINT_ID, body_inertia, pin.SE3.Identity())
- box_shape = fcl.Box(0.5, 0.3, 0.4)
+ model_mobile_base.appendBodyToJoint(
+ MOBILE_BASE_JOINT_ID, body_inertia, pin.SE3.Identity()
+ )
+ box_shape = fcl.Box(0.5, 0.3, 0.4)
body_placement = pin.SE3.Identity()
- geometry_mobile_base = pin.GeometryObject("box_shape", MOBILE_BASE_JOINT_ID, box_shape, body_placement.copy())
+ geometry_mobile_base = pin.GeometryObject(
+ "box_shape", MOBILE_BASE_JOINT_ID, box_shape, body_placement.copy()
+ )
- geometry_mobile_base.meshColor = np.array([1., 0.1, 0.1, 1.])
+ geometry_mobile_base.meshColor = np.array([1.0, 0.1, 0.1, 1.0])
geom_model_mobile_base.addGeometryObject(geometry_mobile_base)
# have to add the frame manually
# it's tool0 because that's used everywhere
- model_mobile_base.addFrame(pin.Frame('tool0',MOBILE_BASE_JOINT_ID,0,joint_placement.copy(),pin.FrameType.JOINT) )
+ model_mobile_base.addFrame(
+ pin.Frame(
+ "tool0",
+ MOBILE_BASE_JOINT_ID,
+ 0,
+ joint_placement.copy(),
+ pin.FrameType.JOINT,
+ )
+ )
data = model_mobile_base.createData()
- return model_mobile_base, geom_model_mobile_base.copy(), geom_model_mobile_base.copy(), data
+ return (
+ model_mobile_base,
+ geom_model_mobile_base.copy(),
+ geom_model_mobile_base.copy(),
+ data,
+ )
+
# TODO:
# try building the mobile base as a rotational joint,
# on top of which is a prismatic joint.
# this way you get the differential-drive basis that you want,
-# and you don't have to manually correct the additional
+# and you don't have to manually correct the additional
# degree of freedom everywhere (jacobian, control etc)
def heron_approximationDD():
- # arm + gripper
+ # arm + gripper
model_arm, collision_model_arm, visual_model_arm, data_arm = get_model()
# mobile base as planar joint (there's probably a better
@@ -383,50 +482,69 @@ def heron_approximationDD():
parent_id = 0
# TEST
joint_placement = pin.SE3.Identity()
- #joint_placement.rotation = pin.rpy.rpyToMatrix(0, -np.pi/2, 0)
- #joint_placement.translation[2] = 0.2
- MOBILE_BASE_REVOLUTE_ID = model_mobile_base.addJoint(parent_id,
- pin.JointModelRZ(),
- joint_placement.copy(), revolute_joint_name)
+ # joint_placement.rotation = pin.rpy.rpyToMatrix(0, -np.pi/2, 0)
+ # joint_placement.translation[2] = 0.2
+ MOBILE_BASE_REVOLUTE_ID = model_mobile_base.addJoint(
+ parent_id, pin.JointModelRZ(), joint_placement.copy(), revolute_joint_name
+ )
MOBILE_BASE_PRISMATIC_ID = model_mobile_base.addJoint(
- MOBILE_BASE_REVOLUTE_ID,
- pin.JointModelPY(),
- joint_placement.copy(), prismatic_joint_name)
+ MOBILE_BASE_REVOLUTE_ID,
+ pin.JointModelPY(),
+ joint_placement.copy(),
+ prismatic_joint_name,
+ )
# we should immediately set velocity limits.
# there are no position limit by default and that is what we want.
# TODO: put in heron's values
model_mobile_base.velocityLimit[0] = 2
model_mobile_base.velocityLimit[1] = 2
-# model_mobile_base.velocityLimit[2] = 2
+ # model_mobile_base.velocityLimit[2] = 2
# TODO: i have literally no idea what reasonable numbers are here
model_mobile_base.effortLimit[0] = 200
model_mobile_base.effortLimit[1] = 200
-# model_mobile_base.effortLimit[2] = 200
- #print("OBJECT_JOINT_ID",OBJECT_JOINT_ID)
- #body_inertia = pin.Inertia.FromBox(args.box_mass, box_dimensions[0],
+ # model_mobile_base.effortLimit[2] = 200
+ # print("OBJECT_JOINT_ID",OBJECT_JOINT_ID)
+ # body_inertia = pin.Inertia.FromBox(args.box_mass, box_dimensions[0],
# box_dimensions[1], box_dimensions[2])
# pretty much random numbers
# TODO: find heron (mir) numbers
- body_inertia = pin.Inertia.FromBox(30, 0.5,
- 0.3, 0.4)
+ body_inertia = pin.Inertia.FromBox(30, 0.5, 0.3, 0.4)
# maybe change placement to sth else depending on where its grasped
- model_mobile_base.appendBodyToJoint(MOBILE_BASE_PRISMATIC_ID,
- body_inertia, pin.SE3.Identity())
- box_shape = fcl.Box(0.5, 0.3, 0.4)
+ model_mobile_base.appendBodyToJoint(
+ MOBILE_BASE_PRISMATIC_ID, body_inertia, pin.SE3.Identity()
+ )
+ box_shape = fcl.Box(0.5, 0.3, 0.4)
body_placement = pin.SE3.Identity()
- geometry_mobile_base = pin.GeometryObject("box_shape", MOBILE_BASE_PRISMATIC_ID, box_shape, body_placement.copy())
+ geometry_mobile_base = pin.GeometryObject(
+ "box_shape", MOBILE_BASE_PRISMATIC_ID, box_shape, body_placement.copy()
+ )
- geometry_mobile_base.meshColor = np.array([1., 0.1, 0.1, 1.])
+ geometry_mobile_base.meshColor = np.array([1.0, 0.1, 0.1, 1.0])
geom_model_mobile_base.addGeometryObject(geometry_mobile_base)
# have to add the frame manually
- model_mobile_base.addFrame(pin.Frame('base',MOBILE_BASE_PRISMATIC_ID,0,joint_placement.copy(),pin.FrameType.JOINT) )
+ model_mobile_base.addFrame(
+ pin.Frame(
+ "base",
+ MOBILE_BASE_PRISMATIC_ID,
+ 0,
+ joint_placement.copy(),
+ pin.FrameType.JOINT,
+ )
+ )
# frame-index should be 1
- model, visual_model = pin.appendModel(model_mobile_base, model_arm, geom_model_mobile_base, visual_model_arm, 1, pin.SE3.Identity())
+ model, visual_model = pin.appendModel(
+ model_mobile_base,
+ model_arm,
+ geom_model_mobile_base,
+ visual_model_arm,
+ 1,
+ pin.SE3.Identity(),
+ )
data = model.createData()
-
+
# fix gripper
for geom in visual_model.geometryObjects:
if "hand" in geom.name:
--
GitLab