From fffcaa68049b5d0b576567feb18cccda2a001a0a Mon Sep 17 00:00:00 2001
From: gac-S_Changer <gac-S_Changer@PC12288.psi.ch>
Date: Thu, 23 Feb 2017 16:57:12 +0100
Subject: [PATCH]

---
 .../usrapp/ForceDetection/ForceDetection.dtx  |  38 ++++++
 .../usr/usrapp/ForceDetection/comTcp.pgx      | 124 +++++++++++-------
 script/RobotTCP.py                            |   3 +
 3 files changed, 118 insertions(+), 47 deletions(-)

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 @@
     <Data name="n" access="private" xsi:type="array" type="num" size="1" />
     <Data name="arr" access="private" xsi:type="array" type="num" size="10" />
     <Data name="b" access="private" xsi:type="array" type="bool" size="1" />
+    <Data name="s" access="private" xsi:type="array" type="string" size="1" />
+    <Data name="gripper" access="private" xsi:type="array" type="tool" size="30">
+      <Value key="0" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="1" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="2" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="3" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="4" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="5" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="6" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="7" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="8" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="9" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="10" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="11" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="12" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="13" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="14" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="15" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="16" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="17" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="18" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="19" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="20" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="21" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="22" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="23" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="24" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="25" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="26" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="27" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="28" fatherId="flange[0]" ioLink="valve1" />
+      <Value key="29" fatherId="flange[0]" ioLink="valve1" />
+    </Data>
+    <Data name="j" access="private" xsi:type="array" type="jointRx" size="1" />
+    <Data name="p" access="private" xsi:type="array" type="pointRx" size="1">
+      <Value key="0" fatherId="world[0]" />
+    </Data>
+    <Data name="t" access="private" xsi:type="array" type="trsf" size="1" />
   </Datas>
 </Database>
\ 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 = []