From 7d24d7801a6a86495a1b3d86af92cf8c185e2454 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Wed, 4 Dec 2024 01:50:12 +0100
Subject: [PATCH] now it's shared memory all the way. and the path generation
itself got fixed. now we can actually just compute the reference for the
gripper and be done with this part
---
.../__pycache__/managers.cpython-312.pyc | Bin 53810 -> 54489 bytes
python/ur_simple_control/managers.py | 40 ++++++++++++------
.../optimal_control/crocoddyl_mpc.py | 2 +-
.../path_generation/planner.py | 12 ++++--
4 files changed, 38 insertions(+), 16 deletions(-)
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index c2815be629cbe704bc81afe0a7c810441fb93951..0343be7262ccf487703d207bbfbaeb2cdb691335 100644
GIT binary patch
delta 1480
zcmdnAg!$%5X1>$Byj%<n3=HR&`KOECn#d=?7_d>DmC+}KA%!tVH%gb2Aw{5tAxe*l
z!JQ#Ru!SK-Xf+c^Jp)6OK3Gh+g&{=*E@l806K!Eg5rc~vZZ2R<5@B@Od|C9h0OQHc
z`_*=FGrrnfWyHwM=(f4WmYa!j-sTSb8ezr>n^}WTurTsZJ{P{4QD}2jgak9A=;q1M
zF^r5Fn?J+^u`+6Jc20W7&gj3{D2tzov3#>r&L<Yerp=~>YuFi=Zhlmu#mabPGjE+2
zGvl|-5se2K8Cf<<HLEf+@@#f!yUflgy;-F18Z)EM=GPOSGctxvem&8Dv(l7V+>9xk
z_s)-CWn8dXZ0QU}#&w(5FH2=)+&WojrP5^c6<-*4Z`N5^#Kd@T^8D3*8A~?LUGtBX
zNr-jx+)aIqEE5=u1U7qbkz-^_;i_dX;h!uxRaTZgg*}CP4aaI`NLbY{#0x`M3=Acj
zlWQ$x_)-{FGeU%FIchj2JC=%1{<lR~JcXe|7b48SP{RRJB0V`{tIlM;QsK$Vw@NbV
zPd>O+Sl9rjEL($tp-8QUDa&Z`MGN7{H+6(3%U6pD#7n~z<jK`AWtmKF^b%&`shZ?2
zi6m9STEh@8Gr4e^JfrpGi>Kr#Z`dX>`NIqmKIIa7nD!Ljn#mVgM47V8Ci|aJVbbKA
zti0Wyt0;wmfgw{soq=I;(RLk1g~{``r*N<rXXM5w=cY_%-{C5Eivz?<Ni0dc#a57+
zoSl<;i#4S*x1hL)iIIU}vd<1_X6~HC$=N$38967n?y!)&#S7CApO=`MdW*53n3I8l
zK|x{ip&eT6ubCMbiq)7Vf8McEn}>l%YDVB?Zn+2I3LPGIq&2Qfn_iSQ-H>sh<U&x~
z^`O*?L8(`y(>h#wd?w%DDN~<!F)8nQQqjevqAN<p9WGaR<Zq}NZd6<l-{EqdNB$y@
z{6fYHJW5a0wHKFxWK|x>D0TQgk<q*^V|G!-Y(wpdkP8v9S7hQkTzY(;2#8JRnaDFE
z<g$QLhvN-Rtqzxd-%j7_JhB&gWM`z$DPCYWyX*pw+7ku!6}30?jW@&`m~6I-kEi5f
zddUU7vM<bxiWQRscDb`lt9Gzl;gHxoZ`U*?M#ashdu7?#j2Rgiip(ZQ9Pwb(+r03|
z6lO-V$z~_Q7y~Ewo|rOuVzV?y3S+i814EJJ<bydzOvUn(59WwZKEG0U@`ET2mKuh5
zmdS};a+BqAxhG#{5uW^@U1YMsA}%Iys+6C+K1FzP|5TC5{Jz|bETBlA{OGjoWcd>L
z$$C>oC-eJq2nI7~GWiufVPIg;<SUW|Mbq7rl4?32SE+*tD-fZ{$iQ%mH77qYrMO4~
zB%lf+tS5gzX;+`b!oW~Goq>U&f#C)>|8;J;i`;Sx6fbcr-w+m`Za>lfy0FGYVT~)o
zS~rx{m)kG2zpiY1QQ7vgvi$^>D*_5=T6hGm^Qc_pQMt~ebCE~q3Xk3$dDZzfGiz43
ztoL2%dtJx&qK@rlc{@;)$=m=dRzy+FFWg_-Svw=-x~%R+S>4P0dKWnKCiBKhZ=QOJ
znVDM~<Z3w(VLEy7Sx?Ixkkh|ob2^VezfY&nbso8kJaSif6mG!ypp<ikM{%>_Ic`R7
z3y?`RAi{C7`FTZ&^Pu!<&cMLX!tjwvf|cX5nLeu@<A<Ed1?S~OIau{Si}12KGZukV
z6#0Sz+-UQX^Jf^DG#Mx7UNqneVPs$c6%fU3lb2r%WWLS7JNd^&O~EWCMrX!P3}6bZ
F7XY4z)baoT
delta 1082
zcmcb)l6lh-X1>$Byj%<n3=B!#{^@70P2`hc^w_A*%ILz$kRs5+5T(n+;LeaD*usz^
zw3-Q|j)5Uc4=g6!!jK{Y7t;reiMBALh{44SHdinvi7+~DzAgG%fbqoU<7&IO8DDO0
zGGgRrblu!y%gw|%ck=}M8ezu%&Ah=USQz;xUkhK&D7d*PLV}r5Wb^Fk7)D0*%|Bv-
zSQ)i8dndhPXY|`_mBr7*Shm?K=MxKK<7V5!HSCN_HovOSVr9I%S+vfJnepr9gvNu6
zY|P9I48_5decM7dS2v$wV&vZJ(RP`gQEIbH-!*1N@5#3(`)_udG=q^bbo1@WFS!|$
zHy@oJ!OA#)v)s}djErkHZ(o+m$hc**><TeP#$A(TS14@`TN%K_cwqAK)qfd_H!ofD
zkCmy3W%JTaeT*zcCrel-2j<GKrLfhqmGDn~a871&<d#2^uWuHdEVNB((iV})4%;Nu
z)gj@>z)-`qni0Y-(SXnl4A~kC3`J@+Oj%l!H<}4gPAC<doKP*y7cUM|kSABeG<l(g
z2y+To)#QuGk^%^+8rB+yc!|mRr{x)qCo@{gPww*;nLKHmFkg{!i8)Mv3iss3sp6CE
zb%ZDXIVZ%V$uoKPcK^vDJ9HRjCOhs(VPP)L$ep})hwEg4ozg5kIf=!S^>#`!vQPHd
zX~9~|&cMJhdFD<n_E(@_eZ@HW)Xts8Yz#b7GXgJh%iWPvo}J#|(&PU?T%p6`j=0Ko
zal?z^h8r{vn4HMD?w5GcFY$_aQin^A$K-XpWDK${CS_eu%Db49cSR|`!{rK({0&vZ
zjfxB6J6x{w$Y11<U&wfYN9m5b_Tn;-tjc7&-I6?I7t_lw=v91SW>l=2oV?qeT}G{g
z<qC)7=8e0jF)@m5_Sh%O&Zfu6z))m3Is2#wqw?l0N2f3|8cy~+8OG=~dBMpklNanW
zXDXJTT<9e}`9Y5K<h7f5C%@aoIhlh+WHM{L*yQ`CxfF{wGB7Y`vK1*XGBDg?P0cMR
zsl3INo>~&0mtUTkSyH4q+5C*1PdEz$L$M75149GD4SwPN`p)|6{3;juRj%->-B3_j
z&cBfVx`Ne31*^*nHXskk-r*PSukEaz5prEt@1m^UWq$n&9Qs9|m`P@wJkeBSv%y(r
zW^N6T<K;kv!DPD&o@_B7TRu<jy&x{((dp6e*XehKN9G2PKtG7*-|2saM|Sgp3*3y{
z#vncBAi{d`wTp@p=Rk1;GQEZ2Ba;Lx$7eHDRzJoM5tI2Z$%`_x8hqyBWp!o*+h61h
v@~`1$=Syc8CyQLs<?>@>U;w4f;^xWTR{|%`zoID^&BW-;_=y2bfi(dDXdYWS
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index c64cd26..f6731d0 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -25,6 +25,7 @@ from sys import exc_info
from types import NoneType
from os import getpid
from functools import partial
+import pickle
"""
general notes
@@ -1016,23 +1017,33 @@ class ProcessManager:
self.data_queue = Queue()
self.side_process = Process(target=side_function,
args=(args, init_command, self.command_queue, self.data_queue,))
- # both ways, with shared memory for commands
+ # shared memory both ways
+ # one lock because i'm lazy
+ # but also, it's just copy-pasting
+ # we're in python, and NOW do we get picky with copy-pasting???????
if comm_direction == 3:
- self.data_queue = Queue()
# "sending" the command via shared memory
shm_name = "command"
- self.shm = shared_memory.SharedMemory(shm_name, create=True, size=init_command.nbytes)
- self.shared_command = np.ndarray(init_command.shape, dtype=init_command.dtype, buffer=self.shm.buf)
+ self.shm_cmd = shared_memory.SharedMemory(shm_name, create=True, size=init_command.nbytes)
+ self.shared_command = np.ndarray(init_command.shape, dtype=init_command.dtype, buffer=self.shm_cmd.buf)
self.shared_command[:] = init_command[:]
+ # same lock for both
self.shared_command_lock = Lock()
+ # getting data via different shared memory
+ shm_data_name = "data"
+ # size is chosen arbitrarily but 10k should be more than enough for anything really
+ self.shm_data = shared_memory.SharedMemory(shm_data_name, create=True, size=10000)
+ # initialize empty
+ p = pickle.dumps(None)
+ self.shm_data.buf[:len(p)] = p
# the process has to create its shared memory
self.side_process = Process(target=side_function,
- args=(args, init_command, shm_name, self.shared_command_lock, self.data_queue,))
+ args=(args, init_command, shm_name, self.shared_command_lock, self.shm_data,))
if type(side_function) == partial:
self.side_process.name = side_function.func.__name__
else:
self.side_process.name = side_function.__name__ + "_process"
- self.lastest_data = init_value
+ self.latest_data = init_value
self.side_process.start()
if self.args.debug_prints:
@@ -1090,7 +1101,7 @@ class ProcessManager:
#self.command_queue.get_nowait()
self.command_queue.put_nowait(command)
- # TODO: implement
+ # TODO merge with send command under if which depends on comm_direction
def sendCommandViaSharedMemory(self, command):
"""
sendCommandViaSharedMemory
@@ -1109,14 +1120,19 @@ class ProcessManager:
self.shared_command_lock.release()
def getData(self):
- if not self.data_queue.empty():
- self.lastest_data = self.data_queue.get_nowait()
- return copy.deepcopy(self.lastest_data)
+ if self.comm_direction != 3:
+ if not self.data_queue.empty():
+ self.latest_data = self.data_queue.get_nowait()
+ if self.comm_direction == 3:
+ self.shared_command_lock.acquire()
+ self.latest_data = pickle.loads(self.shm_data.buf)
+ self.shared_command_lock.release()
+ return copy.deepcopy(self.latest_data)
def terminateProcess(self):
if self.comm_direction == 3:
- self.shm.close()
- self.shm.unlink()
+ self.shm_cmd.close()
+ self.shm_cmd.unlink()
if self.args.debug_prints:
print(f"i am putting befree in {self.side_process.name}'s command queue to stop it")
self.command_queue.put_nowait("befree")
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index 078883c..d0a17c5 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -221,7 +221,7 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
T_w_e = robot.getT_w_e()
#p = T_w_e.translation[:2]
p = q[:2]
- print("CTRL:", p)
+ #print("CTRL:", p)
# NOTE: this is the actual position, not what the path suggested
# whether this or path reference should be put in is debateable.
# this feels more correct to me.
diff --git a/python/ur_simple_control/path_generation/planner.py b/python/ur_simple_control/path_generation/planner.py
index 38eeaf2..a93d6d8 100644
--- a/python/ur_simple_control/path_generation/planner.py
+++ b/python/ur_simple_control/path_generation/planner.py
@@ -10,6 +10,7 @@ from ur_simple_control.path_generation.starworlds.obstacles import StarshapedPol
import shapely
import yaml
import pinocchio as pin
+import pickle
import matplotlib.pyplot as plt
import matplotlib.collections as plt_col
@@ -721,7 +722,7 @@ def path2D_to_timed_SE3_base_and_ee(args, path_pol, path2D_untimed, max_base_v):
# spitting out path points
#def starPlanner(goal, args, init_cmd, cmd_queue : Queue, data_queue : Queue):
# let's try shared memory
-def starPlanner(goal, args, init_cmd, shm_name, lock : Lock, data_queue : Queue):
+def starPlanner(goal, args, init_cmd, shm_name, lock : Lock, shm_data):
"""
starPlanner
------------
@@ -778,8 +779,13 @@ def starPlanner(goal, args, init_cmd, shm_name, lock : Lock, data_queue : Queue)
# compute the path
path_pol, epsilon = path_gen.update(p, r0, rg, rho, obstacles_star)
# TODO: this is stupid, just used shared memory bro
- if data_queue.qsize() < 1:
- data_queue.put((path_pol, path_gen.target_path))
+ #if data_queue.qsize() < 1:
+ #data_queue.put((path_pol, path_gen.target_path))
+ data_pickled = pickle.dumps((path_pol, path_gen.target_path))
+ lock.acquire()
+ shm_data.buf[:len(data_pickled)] = data_pickled
+ #shm_data.buf[len(data_pickled):] = bytes(0)
+ lock.release()
except KeyboardInterrupt:
shm.close()
--
GitLab