From 18fed61168373bf8793f81c6a5a4c33f831ea31b Mon Sep 17 00:00:00 2001
From: mustaffxx <felipeamorimw@gmail.com>
Date: Thu, 8 Apr 2021 14:22:52 -0300
Subject: [PATCH] add support to get actual joint current
---
urx/urrobot.py | 7 +++++++
urx/urrtmon.py | 17 ++++++++++++++++-
2 files changed, 23 insertions(+), 1 deletion(-)
diff --git a/urx/urrobot.py b/urx/urrobot.py
index 94c87f5..54641db 100644
--- a/urx/urrobot.py
+++ b/urx/urrobot.py
@@ -104,6 +104,13 @@ class URRobot(object):
force += i**2
return force**0.5
+ def get_joint_current(self, wait=True):
+ """
+ return measured joint current
+ if wait==True, waits for next packet before returning
+ """
+ return self.rtmon.getJOINTCurrent(wait)
+
def set_tcp(self, tcp):
"""
set robot flange to tool tip transformation
diff --git a/urx/urrtmon.py b/urx/urrtmon.py
index 9c08133..d3fa1c6 100644
--- a/urx/urrtmon.py
+++ b/urx/urrtmon.py
@@ -47,6 +47,7 @@ class URRTMonitor(threading.Thread):
self._qTarget = None
self._tcp = None
self._tcp_force = None
+ self._joint_current = None
self.__recvTime = 0
self._last_ctrl_ts = 0
# self._last_ts = 0
@@ -140,6 +141,18 @@ class URRTMonitor(threading.Thread):
return tcf_force
getTCFForce = tcf_force
+ def joint_current(self, wait=False, timestamp=False):
+ """ Get the joint current."""
+ if wait:
+ self.wait()
+ with self._dataAccess:
+ joint_current = self._joint_current
+ if timestamp:
+ return self._timestamp, joint_current
+ else:
+ return joint_current
+ getJOINTCurrent = joint_current
+
def __recv_rt_data(self):
head = self.__recv_bytes(4)
# Record the timestamp for this logical package
@@ -178,6 +191,7 @@ class URRTMonitor(threading.Thread):
self._qTarget = np.array(unp[1:7])
self._tcp_force = np.array(unp[67:73])
self._tcp = np.array(unp[73:79])
+ self._joint_current = np.array(unp[43:49])
if self._csys:
with self._csys_lock:
@@ -245,7 +259,8 @@ class URRTMonitor(threading.Thread):
qActual=self._qActual,
qTarget=self._qTarget,
tcp=self._tcp,
- tcp_force=self._tcp_force)
+ tcp_force=self._tcp_force,
+ joint_current=self._joint_current)
def stop(self):
# print(self.__class__.__name__+': Stopping')