From 99cb0ca6e75f8ac29ee2c2affa33a6cdbe2c0a54 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Thu, 23 Jan 2025 19:13:07 +0100
Subject: [PATCH] double serialization is goin on because shared memory in
python is weird and protobuf doesn't delimit the messages by itself. the
message lenth should cut the buffer, but something evil happens and then i
doesn't wokr. idk what any of this is about. the pickling works on time, it's
just stupid.
---
python/convenience_tool_box/freedrive_v2.0.py | 24 +-
python/examples/graz/joint_copier_client.py | 78 ++++++
.../examples/graz/point_impedance_server.py | 231 ++++++++++++++++++
.../__pycache__/managers.cpython-311.pyc | Bin 61911 -> 61103 bytes
python/ur_simple_control/managers.py | 47 +++-
.../ur_simple_control/networking/__init__.py | 6 +-
python/ur_simple_control/networking/client.py | 57 ++++-
.../networking/message_specs.proto | 1 +
.../networking/message_specs_pb2.py | 10 +-
python/ur_simple_control/networking/server.py | 2 -
python/ur_simple_control/networking/util.py | 35 ++-
11 files changed, 438 insertions(+), 53 deletions(-)
create mode 100644 python/examples/graz/joint_copier_client.py
create mode 100644 python/examples/graz/point_impedance_server.py
diff --git a/python/convenience_tool_box/freedrive_v2.0.py b/python/convenience_tool_box/freedrive_v2.0.py
index ea35610..5c7c519 100644
--- a/python/convenience_tool_box/freedrive_v2.0.py
+++ b/python/convenience_tool_box/freedrive_v2.0.py
@@ -3,23 +3,31 @@ 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,
+)
from ur_simple_control.basics.basics import freedriveUntilKeyboard
import pickle
+
def get_args():
parser = getMinimalArgParser()
- parser.description = 'nothing'
+ parser.description = (
+ "freedrive with keyboard bindings to save poses and joint angles"
+ )
# 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)
- pose_n_q_dict = freedriveUntilKeyboard(args, robot)
- file = open("./pose_n_q_dict.pickle", 'wb')
+ pose_n_q_dict = freedriveUntilKeyboard(args, robot)
+ file = open("./pose_n_q_dict.pickle", "wb")
pickle.dump(pose_n_q_dict, file)
file.close()
print(pose_n_q_dict)
@@ -31,9 +39,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/graz/joint_copier_client.py b/python/examples/graz/joint_copier_client.py
new file mode 100644
index 0000000..cd95e82
--- /dev/null
+++ b/python/examples/graz/joint_copier_client.py
@@ -0,0 +1,78 @@
+import pinocchio as pin
+import numpy as np
+from functools import partial
+from ur_simple_control.networking.client import client
+from ur_simple_control.managers import (
+ ProcessManager,
+ getMinimalArgParser,
+ ControlLoopManager,
+ RobotManager,
+)
+
+
+def get_args():
+ parser = getMinimalArgParser()
+ parser.description = "the robot will received joint angles from a socket and go to them in joint space"
+ # add more arguments here from different Simple Manipulator Control modules
+ parser.add_argument("--host", type=str, help="host ip address", default="127.0.0.1")
+ parser.add_argument("--port", type=int, help="host's port", default=7777)
+ args = parser.parse_args()
+ return args
+
+
+def controlLoopExternalQ(robot: RobotManager, receiver: ProcessManager, i, past_data):
+ """
+ controlLoop
+ -----------------------------
+ controller description
+ """
+ breakFlag = False
+ log_item = {}
+ save_past_dict = {}
+
+ q = robot.getQ()
+
+ q_desired = receiver.getData()["q"]
+ q_error = q_desired - q
+ K = 120
+ qd_cmd = K * q_error * robot.dt
+
+ robot.sendQd(qd_cmd)
+
+ log_item["qs"] = q.reshape((robot.model.nq,))
+ log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
+ log_item["q_error"] = q_error.reshape((robot.model.nq,))
+ return breakFlag, save_past_dict, log_item
+
+
+if __name__ == "__main__":
+ args = get_args()
+ robot = RobotManager(args)
+
+ # get expected behaviour here (library can't know what the end is - you have to do this here)
+ if not args.pinocchio_only:
+ robot.stopRobot()
+
+ # VERY important that the first q we'll pass as desired is the current q, meaning the robot won't move
+ # this is set with init_value
+ receiver = ProcessManager(
+ args, client, {"q": robot.q.copy()}, 4, init_value={"q": robot.q.copy()}
+ )
+ log_item = {
+ "qs": np.zeros((robot.model.nq,)),
+ "dqs": np.zeros((robot.model.nv,)),
+ "q_error": np.zeros((robot.model.nq,)),
+ }
+ control_loop = partial(controlLoopExternalQ, robot, receiver)
+ loop_manager = ControlLoopManager(robot, control_loop, args, {}, log_item)
+ loop_manager.run()
+
+ if args.save_log:
+ robot.log_manager.plotAllControlLoops()
+
+ if args.visualize_manipulator:
+ robot.killManipulatorVisualizer()
+
+ if args.save_log:
+ robot.log_manager.saveLog()
+ # loop_manager.stopHandler(None, None)
diff --git a/python/examples/graz/point_impedance_server.py b/python/examples/graz/point_impedance_server.py
new file mode 100644
index 0000000..f525008
--- /dev/null
+++ b/python/examples/graz/point_impedance_server.py
@@ -0,0 +1,231 @@
+# PYTHON_ARGCOMPLETE_OK
+import pinocchio as pin
+import numpy as np
+import copy
+import argparse
+from functools import partial
+from ur_simple_control.clik.clik import (
+ getClikArgs,
+ getClikController,
+ moveL,
+ moveUntilContact,
+)
+from ur_simple_control.managers import (
+ ProcessManager,
+ getMinimalArgParser,
+ ControlLoopManager,
+ RobotManager,
+)
+from ur_simple_control.networking.server import server
+
+
+def getArgs():
+ parser = getMinimalArgParser()
+ parser = getClikArgs(parser)
+ parser.description = "the robot will stay in place, with impedance. it sends it's joint angles over tcp to whomever connects to the port"
+ parser.add_argument(
+ "--kp",
+ type=float,
+ help="proportial control constant for position errors",
+ default=1.0,
+ )
+ parser.add_argument(
+ "--kv", type=float, help="damping in impedance control", default=0.001
+ )
+ parser.add_argument(
+ "--cartesian-space-impedance",
+ action=argparse.BooleanOptionalAction,
+ help="is the impedance computed and added in cartesian or in joint space",
+ default=False,
+ )
+ parser.add_argument(
+ "--z-only",
+ action=argparse.BooleanOptionalAction,
+ help="whether you have general impedance or just ee z axis",
+ default=False,
+ )
+ parser.add_argument("--host", type=str, help="host ip address", default="127.0.0.1")
+ parser.add_argument("--port", type=int, help="host's port", default=7777)
+
+ args = parser.parse_args()
+ return args
+
+
+# feedforward velocity, feedback position and force for impedance
+def controller():
+ pass
+
+
+def controlLoopPointImpedanceServer(
+ args, q_init, sender: ProcessManager, controller, robot: RobotManager, i, past_data
+):
+ breakFlag = False
+ # TODO rename this into something less confusing
+ save_past_dict = {}
+ log_item = {}
+ q = robot.getQ()
+ Mtool = robot.getT_w_e()
+ wrench = robot.getWrench()
+ log_item["wrench_raw"] = wrench.reshape((6,))
+ # deepcopy for good coding practise (and correctness here)
+ save_past_dict["wrench"] = copy.deepcopy(wrench)
+ # rolling average
+ # 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
+ )
+ if not args.z_only:
+ Z = np.diag(np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]))
+ else:
+ Z = np.diag(np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0]))
+ # Z = np.diag(np.ones(6))
+
+ wrench = Z @ wrench
+
+ # this jacobian might be wrong
+ J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
+ dq = robot.getQd()[:6].reshape((6, 1))
+ # get joint
+ tau = J.T @ wrench
+ tau = tau[:6].reshape((6, 1))
+ # compute control law:
+ # - feedback the position
+ # kv is not needed if we're running velocity control
+ vel_cmd = (
+ args.kp * (q_init[:6].reshape((6, 1)) - q[:6].reshape((6, 1)))
+ + args.alpha * tau
+ )
+ # vel_cmd = np.zeros(6)
+ robot.sendQd(vel_cmd)
+ sender.sendCommand({"q": q})
+
+ # immediatelly stop if something weird happened (some non-convergence)
+ if np.isnan(vel_cmd[0]):
+ breakFlag = True
+
+ # log what you said you'd log
+ # TODO fix the q6 situation (hide this)
+ log_item["qs"] = q[:6].reshape((6,))
+ log_item["dqs"] = dq.reshape((6,))
+ log_item["wrench_used"] = wrench.reshape((6,))
+
+ return breakFlag, save_past_dict, log_item
+
+
+def controlLoopCartesianPointImpedanceServer(
+ args,
+ Mtool_init,
+ sender: ProcessManager,
+ clik_controller,
+ robot: RobotManager,
+ i,
+ past_data,
+):
+ breakFlag = False
+ # TODO rename this into something less confusing
+ save_past_dict = {}
+ log_item = {}
+ q = robot.getQ()
+ Mtool = robot.getT_w_e()
+ wrench = robot.getWrench()
+ log_item["wrench_raw"] = wrench.reshape((6,))
+ save_past_dict["wrench"] = copy.deepcopy(wrench)
+ # 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
+ )
+ # good generic values
+ # Z = np.diag(np.array([1.0, 1.0, 2.0, 1.0, 1.0, 1.0]))
+ # but let's stick to the default for now
+ if not args.z_only:
+ Z = np.diag(np.array([1.0, 1.0, 1.0, 10.0, 10.0, 10.0]))
+ else:
+ Z = np.diag(np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0]))
+ # Z = np.diag(np.array([1.0, 1.0, 1.0, 10.0, 10.0, 10.0]))
+ # Z = np.diag(np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0]))
+
+ wrench = Z @ wrench
+
+ J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
+
+ SEerror = Mtool.actInv(Mtool_init)
+ err_vector = pin.log6(SEerror).vector
+ v_cartesian_body_cmd = args.kp * err_vector + args.alpha * wrench
+
+ vel_cmd = clik_controller(J, v_cartesian_body_cmd)
+ robot.sendQd(vel_cmd)
+ sender.sendCommand({"q": q})
+
+ # immediatelly stop if something weird happened (some non-convergence)
+ if np.isnan(vel_cmd[0]):
+ breakFlag = True
+
+ dq = robot.getQd()[:6].reshape((6, 1))
+ # log what you said you'd log
+ # TODO fix the q6 situation (hide this)
+ log_item["qs"] = q[:6].reshape((6,))
+ log_item["dqs"] = dq.reshape((6,))
+ log_item["wrench_used"] = wrench.reshape((6,))
+
+ return breakFlag, save_past_dict, log_item
+
+
+if __name__ == "__main__":
+ args = getArgs()
+ robot = RobotManager(args)
+ clikController = getClikController(args, robot)
+
+ # TODO: and NOTE the weight, TCP and inertial matrix needs to be set on the robot
+ # you already found an API in rtde_control for this, just put it in initialization
+ # under using/not-using gripper parameters
+ # NOTE: to use this you need to change the version inclusions in
+ # ur_rtde due to a bug there in the current ur_rtde + robot firmware version
+ # (the bug is it works with the firmware verion, but ur_rtde thinks it doesn't)
+ # here you give what you're saving in the rolling past window
+ # it's initial value.
+ # controlLoopManager will populate the queue with these initial values
+ save_past_dict = {
+ "wrench": np.zeros(6),
+ }
+ # here you give it it's initial value
+ log_item = {
+ "qs": np.zeros(robot.n_arm_joints),
+ "dqs": np.zeros(robot.n_arm_joints),
+ "wrench_raw": np.zeros(6),
+ "wrench_used": np.zeros(6),
+ }
+ q_init = robot.getQ()
+ Mtool_init = robot.getT_w_e()
+ sender = ProcessManager(args, server, {"q": np.zeros(6)}, 0)
+
+ if not args.cartesian_space_impedance:
+ controlLoop = partial(
+ controlLoopPointImpedanceServer, args, q_init, sender, controller, robot
+ )
+ else:
+ controlLoop = partial(
+ controlLoopCartesianPointImpedanceServer,
+ args,
+ Mtool_init,
+ sender,
+ clikController,
+ robot,
+ )
+
+ loop_manager = ControlLoopManager(
+ robot, controlLoop, args, save_past_dict, log_item
+ )
+ loop_manager.run()
+
+ if not args.pinocchio_only:
+ robot.stopRobot()
+
+ if args.save_log:
+ robot.log_manager.saveLog()
+ robot.log_manager.plotAllControlLoops()
+
+ if args.visualize_manipulator:
+ robot.killManipulatorVisualizer()
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-311.pyc b/python/ur_simple_control/__pycache__/managers.cpython-311.pyc
index 69c68cad978f4dfb5a8e0f4a8ea599a674315448..2f3690a363cf4d9c51db5e28701a6d2339495f7b 100644
GIT binary patch
delta 2920
zcmccqn0ftOX1?XTyj%<n3=Cy;lhTEMOyrYbys}YUhEd!gg&~D8$1ut$)hNptqyU5$
zm`rZu6rG&TDCTIIYZhh3$dJO2B9vpEYY}AuW(((7=2}HrF*0y6q=>XIL|HR2xHF`P
zwlJiKEn{L}Sj_}71%#t)ChudEl(q%SOSUkiNTJEwZT`eq!_Cj5t6P>?T$-4ZS(RE;
z<uUoMn9}4`yy{$>y1E59`6VT(MVp`THgU0eGcYg|S8mReaAahk!MC>JioWmU?UHKj
zGq~4!T+#8`{8aKiKL<Nh%jD;pGZ?RJZqVXjWR%(5shz>f#Ra!zGn*kdBjfkULPk#+
zl{fz|l4N95*eqmnnuXD7@;!^Uj53o2oTWBDvpmesn7DbBV=5z9M3GT`vXS!|#?_ne
zI}3@jtz~3jDE8a@Jz^a*Q;+@T6)`!COg;6J#p4bzI!?Y3x02Cib5nc=E2H~n?$kUc
z#_Y{i>4B__d7IDVxN|a2+ALDm#>9AF^X!VJER3f&H`X$7GQQi~(srJeQFU`jPZBeu
z`{wQa>lhhBH>XY1VP=foJbB7-ZpQr0(F<Z&881xkT^_cXZ&@89<E_oT%YSn-zS!Kk
z*^h})e)Em3S<H+oo1J$~W@I$le0^69BctWyvb_qMOZG4_GTLu0+ndZJ>dL~vP#nd;
zz|g?(L4birpvUqGxBTXJ`*j%^gEz|@6lLTLXJKF{vSnmom>hURdUF0@tI7R`<}yZa
z);-L_$QVD_>c|JiiJP^L3NuZV<%|hqU|^WekjfCnn8Fanl)@OroWha9+`<^elERY8
zn#Bf64IrG#p30HM31We88j~c50^>BM6xJ4&D6SN?7KSM96!sQ|D4rCiU<OUj%@dFR
zV_}kF+dT118?#FdOA1dmIE58$OJPjm%@NO)sO4m2U}C7@T*kn_uo`R{h^S$RhZ|qR
zkOgNikeb}cDKc4Zu7E%dL%cLdDF~<V)v`@CJTK16mcpLGzlLKOGXukF=E({nB9k-E
zi%Hb5E@Nb1Sj`Ah3c|HqHC#0eSu&HSpI7ALLXn$vUWHM1^4+u2lOLRC;g$ud2H`Z8
z$%SWSm`ZFXAFPwAFR=rOK`_)m;x$ZJ3J?(nhAc%GtA-(q6~>NNg0b>MYM2meQUt0P
z7#OgttYNKTh*yT`gQ{T%^B1U0cHE{i`R)Z?byctc0|P^mT!}A?&A^Z%2-N@<%2Jvv
zcu|#6XtMdmNP{AIMg|5=##^E;naL#qNk*=D$@wX%MJ}me>K1QuPG)LeNqljBa&~G-
zCIidlzKeQ{>XY|gJfC<=v9u_@I5W2(CpA7fKd+=HKSwVwwWK`1C_6JRU9YqxGpER(
zk%8eBe=5Y#_!O9_DVfP7A^E<=>CX8nskb<Ci__yl_T6GH&d80=El$71l9QjDJ-O$S
zxdL}l1js)=AmS(k1H&zz%)HE!_~iWD+{C<;qM*r-FKH`4>=7!+OwP_pO^FAqC`-&K
zO)chTU|>*Cm@Ir*N4^o1TWpv?x#fn4L<dU`=M6cX4wfq%(jV9uc%&xBT()I}u;yI0
zX8phg61#Od8YCv6IN9RLqk6FHjKE9Wau>MeZt(bDKt(r{EiNcqTvWEYqHNXSa)n3X
zhK#}$8H4LGc9&%AcGO&y@wp=7)8W$N`#@Z+!{aL(gN)`CY10iE2TCpkg<X&izbGAk
zMLN90rN`$h7lV?<5}6fQ7Zok9C|X|Nk-K5(-Qnrd<8y^a=7Oy0+~f;NCKu&QugIBR
zkTt!?V|G_UW+vMlwhosb&$|lhD{61(8*hj?U~wYkOymW<@DI$4ijj;T89?M05II@o
z>c+_r?sGEEnaugbn=6GW8=NwVq$VpqSDI{aO=z;iOy0@sr*bgYm2gh}cwbr>T=Rex
zF(3*8NRlsR@?vOWtYMrSkRT?O1xb?(3=GJUlP^A3nq1J!$HT}_!<fRH!raOv$uQY5
zQ-lw~v4Ny}&dJ;_^mtR4gBd_rlO?f^d9(kuP(}qV1t`c&Qvk($d`f0fYH~?tex8D@
zt%9-D<iKkpleHi7Pj<)>nY?Bj*W?Y`1U4%?+{GAO^b?eDK-Hq3CQnfiBLhQGFepv3
z78Yk#rQYHyC@qQ4%P&vNEGg0escZ)kXF)_HNNqHThyiI}PfRW>%`8g2#a@(}lbTqZ
zsyX@Wb&L87phR^AQUq%;@CaPtmRq2BiCg&sxAF~s;Vb-#*ZI{i@vC3t*Sx~7xx(=h
zzwQNo-5UZTR|J%<3+P@F&|Q&!QNZkqfY}8Bvj^P#NZRiTi_c)6XgkGrsnZg#OTu~=
zVCaUh)D+tVE>~2{uc+|aUJ|yw07H`}JQJ0?p{lbY5CoS*ffAwo0!9$L$fE?3<-5S6
zcY#Om12+SY$^~xa$!{M^izP9Eiv1dfEJ*Y+FfgPrW;1M-dt$;k`R)ZCz7!@%R2F5`
zFl6yej(?yx`PLK3$<c|NOj%r$FY1V~)iQ!*xL@comB>!sST3cO!ko<lGOmcdhOver
z9?S;uOXNW$1ZN3MHhds5`KAtEeK3P2iytJaK;=;psA|v@ghW>;DD+uVa|=o;Z*irk
z!lNr47BxkMAXXe$Sx$apN^wyvNFWMC6oH6T5Rn2Rv_M1<D0YJx7#K8}z-7xV4!*q9
z^7xd*lEh+=#R`*OK2?|e2#Q!XMsR5Xi5J03+)5X?l_qOG3t)UYx$2pA{SAl&EWUXJ
zdOWZ5$X?=+y~rbfg-8AZkNj6Y1_`+-H49vp_*_vjxhQUWMclN*;|3O$clm`TSajC*
z)Xpfp#IJCHU*U$b_5_wI0t#O_7z9LmYG;I8k=4D(uXlxC?*fNjkv#(g!{)uub~3Sn
z({#k-nwJU!At3Life26<FDjV4=A|bi_vUXeD;ODhCl?%%RK3MkQdy9hmwt;iG%qth
zugDFg7?I{PK|xb7dERTq`c#nrJHQFMfdK+PFiEg-d|&_(W*&^JevBU&uu*YLa;*9b
z+?M!k5ZO?)rRsvA;}sRB4-9gw`sjocqXbA0PMGkqIx~J?0Fjf!-pDfAZ!URrjgbu$
zwMDg)z2D8?n!?Dy0II!<UvGZ<?gZoHH6I>%mM}4bbm68xGJxg2K&TJcC~$ZJ02U)l
AnE(I)
delta 3793
zcmZ2~m-+f*X1?XTyj%<n3=F68`_m_XoyaG_cxI!z3?si`s$rH9NCJcx7*BR&6`h>V
zD5h?bYZ_(B$dJO2B9vp6YaV3|W(((7<XT2qGBR*7q=>XIL|IK<!zd|j&BWl&kRsl~
zkRq{+iGg7?6UZzOj<T8jfKigscJnXB8g40ZUEQ+G;?l&N%&Jt~+{C=hg3_GClKi47
zkIC=FlqN6dRTmY})h$X*%+W2$%uUrT$jL7$$;?aNe1o@%i_MFHfuXp1bAp5;BgX{^
z!@U|;tRg0_kW}NiAfdOl;);3b=BtwL`Gti*`r|?R<6-(IpV6Gbcy)8C76&7v`sO_C
z3|3K5xI^L*4%z&`fSZx=+vGQfPZ{+#Uow(pWYpgL#`rV~qvhls7H=8VC%<x%+I+zB
zFgs(y<`&0PMzDw?qt;{|=QWJ0Ht%#65@lP%$iPq>u=!%dI%ei8+^U-!V{#aoukdS5
zejj^)(P8q2xRs2~o1@}8SQ*_mKTgSGV$9kclpe^+n7esNjyosg#LaI?+n5;lZ!WHQ
z%EEYRb7U<OC*#}AF>U8r8C5px^dvDex^3?7U&qK8ve|B;4l`rS=IklQxf$~|8!w1q
zWjw!m!qO{@j5jAwSQ@r@_wuFOjL$bGZuVnh)Y`mZYZf!3@@DCslNlKeH?QAS!^mhc
z*>A7HW}iKbjEr`h{q`m^iMp^bFce2IFfcSQd=OyZ5$LhJ!mYLW*nV9`#-Pn#4u~=`
zhE3)=^oucSGwWd<M#i|wLPtI@PT0(RRG4Y9y$h$83j+hgbcR%hD8>|qD5ey~DCQKd
z6y_GjD3%nKRMs@66xJ4&C^jgYt%W6uJ%zo6A&Mh~qlF=gGljE-A&M)7DVRZ%dvo>i
ze=JOkST<LmX=7$8(Vtv>QCiUel&By$8=T0B#A}$cBq1UU3|Uf>6TC&N;-z5%c_KAT
zSyC`w3QrXS149i%7As5yp|Xaxh9O=Cq@8Jj>|{nW^~t?@T<UT#g++2D_AoXBLkcgl
zAz9Ls9iFN%Y4S~;c`0(T%4Ho!g~@@J&rfc=V$R4pdFK^5#(>E;u4uCsb22b6Ojf?C
zBijH9<uoQxDBlo~=wRvLydkI4!E%K|`T@7(<PTRxCpTZ!U<R=_T(t(%AFf6-KM+@#
z9B}Q?q#LUz>y>jdR!p{wGv!TT%m%xwNT!A%YqD*k{A8V*LcE|L1Yu3)#AwFNk+(t_
zCqFpNp;%<hz`)?QlDR05k%8eBYhiI_Rq8FSg3^-sy!`US%#xLpP3~FvYy-K@8IlG>
z7<dG(aLZojR=&ioe34uA3b*PC#!K9q7q~TV@CaPtk-N^Ld5K4Jh5AJvqboc{7hvcE
zGXsy-1#ZpBGUd|vT&fLnB?xOWC)%+wFmNeAL2+tcid#`?aYkx!iF1B#Zem`_<d3&S
zC+lQ!PVT*xR9}>;keryOkd&&BpO;gqV5pD`QKXQWr%;lSs!&*(TAHc@H!d+RMWG}k
zu|y%WL?N>ntRg3|B(=B%uCFq`RG~aEuSB6FUm>%&xHMG{uDLifFF94gIX|zYC_l$1
zKfk~?u|6*`J+%mCRBmE%NotWoewspJPL4uBQGRl2adB!fs`Z&A3Pq)P5WCYr8j4GF
z6ms*6OF;TSPA<vJO$EEGI47|z6=n?F73KMP>Lm&V`2`>~^>BA5mlhSJ=9T1Bf_x6~
zwL)<|s%ycHMHrNslcSIUvZ5qkp|l_+u_RR?vjo+#B^jB;VBh6~f;c5VFI6F_QXw_B
zprkT0FI}Oe9u~w3kdRHyQz$3}7uFzWfP8}*yhwqWkzbmV0`dyjj-*tD;{2Sl)D(sC
z%#sX+;*7+i)D(r>)ZF}{N_z#kn=4a`brcelit}?yOHy+x_39OpN=rZik*AQUke8a8
z0*bcsqRf(1h1`5l+Q?4<g=Hd0Ix!b+OnGKb4%q$q3Mu&tMVaXtB?@`@<vL(rWR@tD
zgWLuR+Jc<SWKg((e3A?@3vNWBLQ-alLOs})k_?aqCF;cr<@rS=89E9%`NbgPGeJT{
zsfj6xNtrpBC6x+k`9*L8^78X^lk;;6N<m4cI5{&Fl(<TZQ;Uibfs~mB@|!|Ier8^Y
zLV02_!gnACr6m^EL(^_bs*VCU9KkLJg=$hNC<Y2DOEU8F6cY0)%M&Y+%!PR|DODjo
zKOY=wi76=xiFpe71tppJc_535Q*%<2OBBj8QcE&Y5$;X|g$pPjg3?7jG@{Bf6BQDn
znN1<T2pWI!koeO9MH1ZK5NCqYUQ((8I7NYTF3hV%scHE|xrv~_0A<kpe1)9+a=2!V
z%o0$BNJ>n~sZ=P>&#PB20VT|&)a1m{;#37lq~xb5B<B}Y>Vl#G6w?aHsYNA;nRz*t
zaC5++Q=FNemzkEC49e}Gm;fial6+8TDP-oAq!y(XgPI)*xrrr3nH3881*t`epukZ=
z1mxtZwSx7<kg^99CD0NevlyNYic3n<(iAfDQ1cbo%ODMqJPuNyUr+#YFsNVx$9hq!
zda*)rer{?>MrK|*+~pbwT^W!BQJ<)gl#`#F4bq(qDtOZJiy(=iBwrynF*{YEI5n?0
zRTJUr^86xDiI-Rm${VQ)5I3Z!mVi8xs*qfnoRg}MpOd1Hl30=mjy#3L^h9JYl&7i}
zr7Gm*mw=K>eP(WE707as-5}SMCn5s6C<#>66s0jTFcf8inna-LT0x=6mVtrcmN1sm
zy-FznQf~Stf{QCXENTuT)l4=FjyS8P2Y9O{R!G%UWX~{p&J78Uf8Zud3FGAXCQ?i#
zOp`A%i%f32$v^qiL{YIUW^m=oz`y`$ZWJ?lF*Gqwwwa|g`GK||Cu0g@3S%piB*Wwd
z`67G}jt#VlF}ZVs+~fz^0+VgGv9rElWMEL3eD1c+X1P1N8SDKtIf^tG7#ND$LBv@Q
z5e^EdNDvVPs+-vplM72Ti&Af~7p3N;CKjh+2BH)wfS^@lSY~1{xa9N&7o1g^Si%ya
zv>sXQGf<e;gKPV*VhjQz6Wlv%duo?5E#bPvuXO>2Zt#os)GlDTqF{VQ!SWKn<pmhJ
z!NGHdL*Y7y#w8AoiyT^4IJ7QsXx&iMSRn|4OQgW9ni-BDc#%U6qFNM9`3111$?F$`
znhJX+N=-h0!*KG3`~N04Hi%DNna@3W*F(X2CcmPe3=9mKd_@7EPznZx5-Yeuy2X{A
z3Twd>#e*b4L0FUvV#R=!<>V)(6c<H<1R_909*7761$!`vI05QeFfuSOK$9nXdTNPF
zVo73^7$O)a7d+yT{0<7M`QWg+!Oee#TW*2kC2r*l+{%-uJ_=yGJNeloZRWfD!keof
zpJQSJJ2Pyu|1$-FAdr1=AOh6zD$1B#|ICw-ar4P%6^sVlMU5agOal?qLBtFY5ep*R
zKsqu(tZWb~A4HT)Zg`<sp8|5o0<c3G7$EQilLRZr2L=#frqAlf_<;d~%wgnaHD2KI
zfq|RV7@d$(U^QOh@_|7CB!x=2G4iu&e_((UN*sKw`X3lLV1&qIv6t$McAM>9USnj8
ynJo2s23H>=0|O{Piyv*?^ZEqi<leV8J+qk@of$tc;HEx8<i0>CtSXB@jc@=&UY&&i
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 7aa7927..e09c5dc 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -101,9 +101,9 @@ def getMinimalArgParser():
help="frequency of the control loop", default=500)
parser.add_argument('--fast-simulation', action=argparse.BooleanOptionalAction, \
help="do you want simulation to as fast as possible? (real-time viz relies on 500Hz)", default=False)
- parser.add_argument('--visualize-manipulator', action=argparse.BooleanOptionalAction, \
+ parser.add_argument('--visualizer', action=argparse.BooleanOptionalAction, \
help="whether you want to visualize the manipulator and workspace with meshcat", default=True)
- parser.add_argument('--real-time-plotting', action=argparse.BooleanOptionalAction, \
+ parser.add_argument('--plotter', action=argparse.BooleanOptionalAction, \
help="whether you want to have some real-time matplotlib graphs (parts of log_dict you select)", default=True)
parser.add_argument('--gripper', type=str, \
help="gripper you're using (no gripper is the default)", \
@@ -240,7 +240,7 @@ class ControlLoopManager:
for key in log_item:
self.log_dict[key] = []
- if self.args.real_time_plotting:
+ if self.args.plotter:
self.plotter_manager = ProcessManager(args, realTimePlotter, log_item, 0)
@@ -285,7 +285,7 @@ class ControlLoopManager:
# so this should be an argument that is use ONLY if we're in simulation
if i % 20 == 0:
# don't send what wasn't ready
- if self.args.visualize_manipulator:
+ if self.args.visualizer:
if self.robot_manager.robot_name != "yumi":
self.robot_manager.visualizer_manager.sendCommand({"q" : self.robot_manager.q,
"T_w_e" : self.robot_manager.getT_w_e()})
@@ -304,7 +304,7 @@ class ControlLoopManager:
pass
# TODO: here call robot manager's update ellipses function
- if self.args.real_time_plotting:
+ if self.args.plotter:
# don't put new stuff in if it didn't handle the previous stuff.
# it's a plotter, who cares if it's late.
# the number 5 is arbitrary
@@ -339,7 +339,7 @@ class ControlLoopManager:
######################################################################
# for over
######################################################################
- if self.args.real_time_plotting:
+ if self.args.plotter:
self.plotter_manager.terminateProcess()
if self.args.save_log:
self.robot_manager.log_manager.storeControlLoopRun(self.log_dict, self.controlLoop.func.__name__, self.final_iteration)
@@ -385,10 +385,10 @@ class ControlLoopManager:
self.robot_manager.log_manager.storeControlLoopRun(self.log_dict, self.controlLoop.func.__name__, self.final_iteration)
self.robot_manager.log_manager.saveLog()
- if self.args.real_time_plotting:
+ if self.args.plotter:
self.plotter_manager.terminateProcess()
- if self.args.visualize_manipulator:
+ if self.args.visualizer:
self.robot_manager.visualizer_manager.terminateProcess()
# TODO: this obviously only makes sense if you're on ur robots
@@ -528,7 +528,7 @@ class RobotManager:
# start visualize manipulator process if selected.
# has to be started here because it lives throughout the whole run
- if args.visualize_manipulator:
+ if args.visualizer:
side_function = partial(manipulatorVisualizer, self.model, self.collision_model, self.visual_model)
self.visualizer_manager = ProcessManager(args, side_function, {"q" : self.q.copy()}, 0)
if args.visualize_collision_approximation:
@@ -563,7 +563,7 @@ class RobotManager:
q.append(0.0)
q = np.array(q)
self.q = q
- if args.visualize_manipulator:
+ if args.visualizer:
self.visualizer_manager.sendCommand({"q" : q})
@@ -1051,7 +1051,7 @@ class RobotManager:
# NOTE i'm not deepcopying this on purpose
# but that might be the preferred thing, we'll see
self.Mgoal = Mgoal
- if self.args.visualize_manipulator:
+ if self.args.visualizer:
# TODO document this somewhere
self.visualizer_manager.sendCommand(
{"Mgoal" : Mgoal})
@@ -1098,7 +1098,7 @@ class RobotManager:
NOTE: this function does not change internal variables!
because it shouldn't - it should only update the visualizer
"""
- if self.args.visualize_manipulator:
+ if self.args.visualizer:
self.visualizer_manager.sendCommand(viz_dict)
else:
if self.args.debug_prints:
@@ -1154,14 +1154,19 @@ class ProcessManager:
self.args = args
self.comm_direction = comm_direction
+ # send command to slave process
if comm_direction == 0:
self.command_queue = Queue()
self.side_process = Process(target=side_function,
args=(args, init_command, self.command_queue,))
+ # get data from slave process
if comm_direction == 1:
self.data_queue = Queue()
self.side_process = Process(target=side_function,
args=(args, init_command, self.data_queue,))
+ # share data in both directions via shared memory with 2 buffers
+ # - one buffer for master to slave
+ # - one buffer for slave to master
if comm_direction == 2:
self.command_queue = Queue()
self.data_queue = Queue()
@@ -1190,6 +1195,7 @@ class ProcessManager:
# 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.shm_data,))
+ # networking client (server can use comm_direction 0)
if comm_direction == 4:
from ur_simple_control.networking.util import DictPb2EncoderDecoder
self.encoder_decoder = DictPb2EncoderDecoder()
@@ -1199,6 +1205,12 @@ class ProcessManager:
# NOTE: size is max size of the recv buffer too,
# and the everything blows up if you manage to fill it atm
self.shm_msg = shared_memory.SharedMemory(shm_name, create=True, size=1024)
+ # need to initialize shared memory with init value
+ # NOTE: EVIL STUFF SO PICKLING ,READ NOTES IN networking/client.py
+ #init_val_as_msg = self.encoder_decoder.dictToSerializedPb2Msg(init_value)
+ #self.shm_msg.buf[:len(init_val_as_msg)] = init_val_as_msg
+ pickled_init_value = pickle.dumps(init_value)
+ self.shm_msg.buf[:len(pickled_init_value)] = pickled_init_value
self.lock = Lock()
self.side_process = Process(target=side_function,
args=(args, init_command, shm_name, self.lock))
@@ -1262,8 +1274,17 @@ class ProcessManager:
if self.comm_direction == 4:
self.lock.acquire()
#data_copy = copy.deepcopy(self.shm_msg.buf)
- self.latest_data = self.encoder_decoder.serializedPb2MsgToDict(self.shm_msg.buf, self.msg_code)
+ # REFUSES TO WORK IF YOU DON'T PRE-CROP HERE!!!
+ # MAKES ABSOLUTELY NO SENSE!!! READ MORE IN ur_simple_control/networking/client.py
+ # so we're decoding there, pickling, and now unpickling.
+ # yes, it's incredibly stupid
+ #new_data = self.encoder_decoder.serializedPb2MsgToDict(self.shm_msg.buf, self.msg_code)
+ new_data = pickle.loads(self.shm_msg.buf)
self.lock.release()
+ if len(new_data) > 0:
+ self.latest_data = new_data
+ #print("new_data", new_data)
+ #print("self.latest_data", self.latest_data)
#self.latest_data = self.encoder_decoder.serializedPb2MsgToDict(data_copy, self.msg_code)
return copy.deepcopy(self.latest_data)
diff --git a/python/ur_simple_control/networking/__init__.py b/python/ur_simple_control/networking/__init__.py
index df760a3..c466656 100644
--- a/python/ur_simple_control/networking/__init__.py
+++ b/python/ur_simple_control/networking/__init__.py
@@ -1,3 +1,3 @@
-from client import *
-from server import *
-from util import *
+from .client import *
+from .server import *
+from .util import *
diff --git a/python/ur_simple_control/networking/client.py b/python/ur_simple_control/networking/client.py
index 201dd32..30545be 100644
--- a/python/ur_simple_control/networking/client.py
+++ b/python/ur_simple_control/networking/client.py
@@ -2,6 +2,8 @@ import socket
from multiprocessing import shared_memory
from google.protobuf.internal.encoder import _VarintBytes
from google.protobuf.internal.decoder import _DecodeVarint32
+from ur_simple_control.networking.util import DictPb2EncoderDecoder
+import pickle
def client(args, init_command, shm_name, lock):
@@ -23,6 +25,16 @@ def client(args, init_command, shm_name, lock):
"""
def parse_message(msg_raw):
+ """
+ parse_message
+ -------------
+ here the message is what we got from recv(),
+ and parsing it refers to finding serialized pb2 messages in it
+ NOTE: we only keep the latest message because
+ we're not going to do anything with the missed message.
+ the assumption is that we only get the same kind of message from
+ a sensor or something similar, not files or whatever else needs to be whole
+ """
pos, next_pos = 0, 0
msg_len = len(msg_raw)
msg_in_bytes = b""
@@ -32,20 +44,18 @@ def client(args, init_command, shm_name, lock):
if pos + next_pos > msg_len:
print("NETWORKING CLIENT: BUFFER OVERFLOW, DROPPING MSG!")
break
- # NOTE: we only keep the latest message, we're not going to do anything with the missed message.
- # the assumption is that we only get the same kind of message from a sensor or something similar,
- # not files or whatever.
- # hence we only deserialize the last message
- if pos >= len(msg_raw):
- # why i am encoding the length back in ???
- msg_in_bytes = (
- _VarintBytes(pos + next_pos) + msg_raw[pos : pos + next_pos]
- )
+ msg_in_bytes = _VarintBytes(pos + next_pos) + msg_raw[pos : pos + next_pos]
+ pos += next_pos
+ if pos >= msg_len:
break
return msg_in_bytes
+ # TODO: deltte
+ encoder_decoder = DictPb2EncoderDecoder()
+ msg_code = encoder_decoder.dictToMsgCode(init_command)
+
shm = shared_memory.SharedMemory(name=shm_name)
- host_addr = (args.host_addr, args.host_port)
+ host_addr = (args.host, args.port)
s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
s.connect(host_addr)
if args.debug_prints:
@@ -55,8 +65,33 @@ def client(args, init_command, shm_name, lock):
while True:
msg_raw = s.recv(1024)
msg_in_bytes = parse_message(msg_raw)
+ dict_msg = encoder_decoder.serializedPb2MsgToDict(msg_in_bytes, msg_code)
+
+ # print(
+ # "NETWORKING CLIENT: putting new message in:",
+ # encoder_decoder.serializedPb2MsgToDict(msg_in_bytes, 1),
+ # )
+ # TODO: I CAN'T READ THIS WITHOUT pre-CROPPING THE MESSAGE
+ # WHEN SENDING TO serializedPb2MsgToDict!!!!!!!!!!!
+ # THE FUNCTION CAN CORRECTLY DECODE THE LENGTH OF THE MESSAGE,
+ # AND I CROP IT THERE, BUT FOR SOME REASON IT REFUSES TO COORPERATE
+ # so fck it i'm reserializing with pickling to avoid wasting more time
+ # shm.buf[: len(msg_in_bytes)] = msg_in_bytes[:]
+ dict_msg_pickle = pickle.dumps(dict_msg)
lock.acquire()
- shm.buf[: len(msg_in_bytes)] = msg_in_bytes
+ shm.buf[: len(dict_msg_pickle)] = dict_msg_pickle
+ # print("NETWORKING CLIENT: message in bytes length", len(msg_in_bytes))
+ # mem = shm.buf[: len(msg_in_bytes)]
+ ## print(mem)
+ # print("NETWORKING CLIENT: i can read back from shm:")
+ # print("NETWORKING_CLIENT", encoder_decoder.serializedPb2MsgToDict(mem, 1))
+ # NOTE: this works just fine, but not, but you have to crop here with len(msg_in_bytes),
+ # even though the message begins with the int which is the same number.
+ # but cropping in serializedPb2MsgToDict refuses to work
+ # print(
+ # "NETWORKING_CLIENT",
+ # encoder_decoder.serializedPb2MsgToDict(shm.buf[: len(msg_in_bytes)], 1),
+ # )
lock.release()
except KeyboardInterrupt:
s.close()
diff --git a/python/ur_simple_control/networking/message_specs.proto b/python/ur_simple_control/networking/message_specs.proto
index 730a91b..3048168 100644
--- a/python/ur_simple_control/networking/message_specs.proto
+++ b/python/ur_simple_control/networking/message_specs.proto
@@ -1,4 +1,5 @@
edition = "2023";
+// syntax = "proto2";
message joint_angles { repeated double q = 1; }
diff --git a/python/ur_simple_control/networking/message_specs_pb2.py b/python/ur_simple_control/networking/message_specs_pb2.py
index 5ae3bf8..3d2d21d 100644
--- a/python/ur_simple_control/networking/message_specs_pb2.py
+++ b/python/ur_simple_control/networking/message_specs_pb2.py
@@ -2,7 +2,7 @@
# Generated by the protocol buffer compiler. DO NOT EDIT!
# NO CHECKED-IN PROTOBUF GENCODE
# source: message_specs.proto
-# Protobuf Python Version: 5.29.2
+# Protobuf Python Version: 5.28.3
"""Generated protocol buffer code."""
from google.protobuf import descriptor as _descriptor
from google.protobuf import descriptor_pool as _descriptor_pool
@@ -12,8 +12,8 @@ from google.protobuf.internal import builder as _builder
_runtime_version.ValidateProtobufRuntimeVersion(
_runtime_version.Domain.PUBLIC,
5,
- 29,
- 2,
+ 28,
+ 3,
'',
'message_specs.proto'
)
@@ -24,7 +24,7 @@ _sym_db = _symbol_database.Default()
-DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x13message_specs.proto\"\x19\n\x0cjoint_angles\x12\t\n\x01q\x18\x01 \x03(\x01\"8\n\x08wrenches\x12\x13\n\x0bwrench_mine\x18\x01 \x03(\x01\x12\x17\n\x0fwrench_estimate\x18\x02 \x03(\x01\x62\x08\x65\x64itionsp\xe8\x07')
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x13message_specs.proto\"\x19\n\x0cjoint_angles\x12\t\n\x01q\x18\x01 \x03(\x01\"3\n\x08wrenches\x12\x0e\n\x06wrench\x18\x01 \x03(\x01\x12\x17\n\x0fwrench_estimate\x18\x02 \x03(\x01\x62\x08\x65\x64itionsp\xe8\x07')
_globals = globals()
_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, _globals)
@@ -34,5 +34,5 @@ if not _descriptor._USE_C_DESCRIPTORS:
_globals['_JOINT_ANGLES']._serialized_start=23
_globals['_JOINT_ANGLES']._serialized_end=48
_globals['_WRENCHES']._serialized_start=50
- _globals['_WRENCHES']._serialized_end=106
+ _globals['_WRENCHES']._serialized_end=101
# @@protoc_insertion_point(module_scope)
diff --git a/python/ur_simple_control/networking/server.py b/python/ur_simple_control/networking/server.py
index 13c2340..f3bd566 100644
--- a/python/ur_simple_control/networking/server.py
+++ b/python/ur_simple_control/networking/server.py
@@ -1,6 +1,4 @@
import socket
-import time
-import message_specs_pb2
from google.protobuf.internal.encoder import _VarintBytes
from ur_simple_control.networking.util import DictPb2EncoderDecoder
diff --git a/python/ur_simple_control/networking/util.py b/python/ur_simple_control/networking/util.py
index 46397f3..909a3ba 100644
--- a/python/ur_simple_control/networking/util.py
+++ b/python/ur_simple_control/networking/util.py
@@ -1,6 +1,7 @@
-import message_specs_pb2
+import ur_simple_control.networking.message_specs_pb2 as message_specs_pb2
from google.protobuf.internal.encoder import _VarintBytes
from google.protobuf.internal.decoder import _DecodeVarint32
+import numpy as np
"""
to make the data API uniform across the library (dictionaries),
@@ -70,19 +71,19 @@ class DictPb2EncoderDecoder:
"""
# pb2_msg = int_to_message[msg_code]
msg_code = self.dictToMsgCode(dict_msg)
- if msg_code == 0:
+ if msg_code == 1:
pb2_msg = message_specs_pb2.joint_angles()
# if i could go from string to class atribute somehow
# that'd be amazing
# TODO: see if that's possible
- pb2_msg.q = dict_msg["q"]
+ pb2_msg.q.extend(dict_msg["q"])
if msg_code == 6:
pb2_msg = message_specs_pb2.wrenches()
# if i could go from string to class atribute somehow
# that'd be amazing
# TODO: see if that's possible
- pb2_msg.wrench = dict_msg["wrench"]
- pb2_msg.wrench_estimate = dict_msg["wrench_estimate"]
+ pb2_msg.wrench.extend(dict_msg["wrench"])
+ pb2_msg.wrench_estimate.extend(dict_msg["wrench_estimate"])
# NOTE: possible extension:
# prepend the message code as well so that i can send different
@@ -115,20 +116,32 @@ class DictPb2EncoderDecoder:
different messages over the same socket.
"""
dict_msg = {}
- if msg_code == 0:
+ # who knows what goes on in the rest of the shared memory
+ # protobuf is not self delimiting
+ # so the first value always have to the length,
+ # and we only read pass just the actual message to ParseFromString()
+ next_pos, pos = 0, 0
+ next_pos, pos = _DecodeVarint32(pb2_msg_in_bytes, pos)
+ # print("did decode the int")
+ # print("pos", pos, "next_pos", next_pos)
+ pb2_msg_in_bytes_cut = pb2_msg_in_bytes[pos : pos + next_pos]
+ if msg_code == 1:
pb2_msg = message_specs_pb2.joint_angles()
- pb2_msg.ParseFromString(pb2_msg_in_bytes)
+ # pb2_msg.ParseFromString(pb2_msg_in_bytes[pos : pos + next_pos])
+ # print("msg", pb2_msg)
+ pb2_msg.ParseFromString(pb2_msg_in_bytes_cut)
+ # print("msg", pb2_msg)
# if i could go from string to class atribute somehow
# that'd be amazing
# TODO: see if that's possible
- dict_msg["q"] = pb2_msg.q
+ dict_msg["q"] = np.array(pb2_msg.q)
if msg_code == 6:
pb2_msg = message_specs_pb2.wrenches()
- pb2_msg.ParseFromString(pb2_msg_in_bytes)
+ pb2_msg.ParseFromString(pb2_msg_in_bytes_cut)
# if i could go from string to class atribute somehow
# that'd be amazing
# TODO: see if that's possible
- dict_msg["wrench"] = pb2_msg.wrench
- dict_msg["wrench_estimate"] = pb2_msg.wrench_estimate
+ dict_msg["wrench"] = np.array(pb2_msg.wrench)
+ dict_msg["wrench_estimate"] = np.array(pb2_msg.wrench_estimate)
return dict_msg
--
GitLab