diff --git a/robot/MXLAB/Controller1/usr/usrapp/ForceDetection/ForceDetection.dtx b/robot/MXLAB/Controller1/usr/usrapp/ForceDetection/ForceDetection.dtx
index d4e1748..18d281e 100644
--- a/robot/MXLAB/Controller1/usr/usrapp/ForceDetection/ForceDetection.dtx
+++ b/robot/MXLAB/Controller1/usr/usrapp/ForceDetection/ForceDetection.dtx
@@ -86,5 +86,43 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/robot/MXLAB/Controller1/usr/usrapp/ForceDetection/comTcp.pgx b/robot/MXLAB/Controller1/usr/usrapp/ForceDetection/comTcp.pgx
index 66b49c4..876f3f1 100644
--- a/robot/MXLAB/Controller1/usr/usrapp/ForceDetection/comTcp.pgx
+++ b/robot/MXLAB/Controller1/usr/usrapp/ForceDetection/comTcp.pgx
@@ -83,72 +83,102 @@
break
case "get_var","get_bool"
- if cmd == "get_var"
- ret = getData(args[0], aux)
- else
- ret = getData(args[0], auxb)
- endIf
- switch(ret)
- case -1
- ex = "The variable does not exists"
- break
- case -2
- ex = "The variable library does not exists"
- break
- case -3
- ex = "The index is out of range"
- break
- case -4
- ex = "The data's type does not match the variable's type"
- break
- default
if cmd == "get_var"
- tx = toString(".4", aux)
+ ret = getData(args[0], aux)
else
- if auxb
- tx = "1"
- else
- tx = "0"
- endIf
- endIf
- break
- endSwitch
- break
-
- case "get_arr"
- toNum(args[1], count, ok)
- tx = ""
- for index = 0 to count
- if cmd == "get_var"
- ret = getData(args[0], aux)
- else
- ret = getData(args[0], auxb)
- endIf
-
- ret = getData(args[0] + "[" + toString("", index )+ "]", aux)
+ ret = getData(args[0], auxb)
+ endIf
switch(ret)
- case -1
+ case -1
ex = "The variable does not exists"
- return
break
case -2
ex = "The variable library does not exists"
- return
break
case -3
ex = "The index is out of range"
- return
break
case -4
ex = "The data's type does not match the variable's type"
- return
break
default
- tx = tx + toString(".4", aux) + "|"
+ if cmd == "get_var"
+ tx = toString(".4", aux)
+ else
+ if auxb
+ tx = "1"
+ else
+ tx = "0"
+ endIf
+ endIf
break
endSwitch
+ break
+
+ case "get_arr"
+ toNum(args[1], count, ok)
+ for index = 0 to count
+ if cmd == "get_var"
+ ret = getData(args[0], aux)
+ else
+ ret = getData(args[0], auxb)
+ endIf
+ ret = getData(args[0] + "[" + toString("", index )+ "]", aux)
+ switch(ret)
+ case -1
+ ex = "The variable does not exists"
+ return
+ break
+ case -2
+ ex = "The variable library does not exists"
+ return
+ break
+ case -3
+ ex = "The index is out of range"
+ return
+ break
+ case -4
+ ex = "The data's type does not match the variable's type"
+ return
+ break
+ default
+ tx = tx + toString(".4", aux) + "|"
+ break
+ endSwitch
endFor
break
+ case "get_str"
+ $exec("s = " + args[0])
+ tx = s
+ break
+ case "get_pnt"
+ $exec("p = " + args[0])
+ $exec("t = " + args[0])
+ tx = tx + toString(".4", p.trsf.x) + "|"
+ tx = tx + toString(".4", p.trsf.y) + "|"
+ tx = tx + toString(".4", p.trsf.z) + "|"
+ tx = tx + toString(".4", p.trsf.rx) + "|"
+ tx = tx + toString(".4", p.trsf.ry) + "|"
+ tx = tx + toString(".4", p.trsf.rz) + "|"
+ break
+ case "get_jnt"
+ $exec("j = " + args[0])
+ tx = tx + toString(".4", j.j1) + "|"
+ tx = tx + toString(".4", j.j2) + "|"
+ tx = tx + toString(".4", j.j3) + "|"
+ tx = tx + toString(".4", j.j4) + "|"
+ tx = tx + toString(".4", j.j5) + "|"
+ tx = tx + toString(".4", j.j6) + "|"
+ break
+ case "get_trf"
+ $exec("t = " + args[0])
+ tx = tx + toString(".4", t.x) + "|"
+ tx = tx + toString(".4", t.y) + "|"
+ tx = tx + toString(".4", t.z) + "|"
+ tx = tx + toString(".4", t.rx) + "|"
+ tx = tx + toString(".4", t.ry) + "|"
+ tx = tx + toString(".4", t.rz) + "|"
+ break
default
//App specific
call onCommandTcp(cmd,args, tx, ex)
diff --git a/script/RobotTCP.py b/script/RobotTCP.py
index d1ed92c..0f5a8fc 100644
--- a/script/RobotTCP.py
+++ b/script/RobotTCP.py
@@ -102,6 +102,9 @@ class RobotTCP(TcpDevice, Stoppable):
for i in range(6): ret.append(float(a[i]))
return ret
+ def set_trf(self, l, name="t"):
+ self.evaluate(name + "={" + str(l[0]) + ","+ str(l[1]) + ","+ str(l[2]) + ","+ str(l[3]) + ","+ str(l[4]) + ","+ str(l[5]) + "}")
+
def get_jnt(self, name="j"):
a = self.execute('get_jnt', name)
ret = []