├── RAPID ├── LOGGER.mod └── SERVER.mod ├── README.md └── abb_node ├── CMakeLists.txt ├── Makefile ├── launch ├── abb_tf.launch └── robot_params.yaml ├── manifest.xml ├── packages ├── abb_communications │ ├── abb.py │ ├── abb_comm.cpp │ ├── abb_comm.h │ └── tool.object.json └── matVec │ ├── HomogTransf.cpp │ ├── HomogTransf.h │ ├── Mat.cpp │ ├── Mat.h │ ├── Polynom.cpp │ ├── Polynom.h │ ├── Quaternion.cpp │ ├── Quaternion.h │ ├── RotMat.cpp │ ├── RotMat.h │ ├── Vec.cpp │ ├── Vec.h │ └── matVec.h ├── src ├── abb_node.cpp └── abb_node.h └── srv ├── robot_GetCartesian.srv ├── robot_GetJoints.srv ├── robot_IsMoving.srv ├── robot_Ping.srv ├── robot_SetCartesian.srv ├── robot_SetComm.srv ├── robot_SetDIO.srv ├── robot_SetJoints.srv ├── robot_SetSpeed.srv ├── robot_SetTool.srv ├── robot_SetTrackDist.srv ├── robot_SetVacuum.srv ├── robot_SetWorkObject.srv ├── robot_SetZone.srv ├── robot_SpecialCommand.srv └── robot_Stop.srv /RAPID/LOGGER.mod: -------------------------------------------------------------------------------- 1 | MODULE LOGGER 2 | 3 | !//////////////// 4 | !GLOBAL VARIABLES 5 | !//////////////// 6 | !PC communication 7 | VAR socketdev clientSocket; 8 | VAR socketdev serverSocket; 9 | PERS string ipController; 10 | PERS num loggerPort:= 5001; 11 | 12 | !Robot configuration 13 | PERS tooldata currentTool; 14 | PERS wobjdata currentWobj; 15 | VAR speeddata currentSpeed; 16 | VAR zonedata currentZone; 17 | 18 | !//Logger sampling rate 19 | !PERS num loggerWaitTime:= 0.01; !Recommended for real controller 20 | PERS num loggerWaitTime:= 0.1; !Recommended for virtual controller 21 | 22 | PROC ServerCreateAndConnect(string ip, num port) 23 | VAR string clientIP; 24 | 25 | SocketCreate serverSocket; 26 | SocketBind serverSocket, ip, port; 27 | SocketListen serverSocket; 28 | TPWrite "LOGGER: Logger waiting for incomming connections ..."; 29 | WHILE SocketGetStatus(clientSocket) <> SOCKET_CONNECTED DO 30 | SocketAccept serverSocket,clientSocket \ClientAddress:=clientIP \Time:=WAIT_MAX; 31 | IF SocketGetStatus(clientSocket) <> SOCKET_CONNECTED THEN 32 | TPWrite "LOGGER: Problem serving an incomming connection."; 33 | TPWrite "LOGGER: Try reconnecting."; 34 | ENDIF 35 | !Wait 0.5 seconds for the next reconnection 36 | WaitTime 0.5; 37 | ENDWHILE 38 | TPWrite "LOGGER: Connected to IP " + clientIP; 39 | ENDPROC 40 | 41 | PROC main() 42 | VAR string data; 43 | VAR robtarget position; 44 | VAR jointtarget joints; 45 | VAR string sendString; 46 | VAR bool connected; 47 | 48 | VAR string date; 49 | VAR string time; 50 | VAR clock timer; 51 | 52 | date:= CDate(); 53 | time:= CTime(); 54 | ClkStart timer; 55 | 56 | connected:=FALSE; 57 | ServerCreateAndConnect ipController,loggerPort; 58 | connected:=TRUE; 59 | WHILE TRUE DO 60 | 61 | !Cartesian Coordinates 62 | position := CRobT(\Tool:=currentTool \WObj:=currentWObj); 63 | data := "# 0 "; 64 | data := data + date + " " + time + " "; 65 | data := data + NumToStr(ClkRead(timer),2) + " "; 66 | data := data + NumToStr(position.trans.x,1) + " "; 67 | data := data + NumToStr(position.trans.y,1) + " "; 68 | data := data + NumToStr(position.trans.z,1) + " "; 69 | data := data + NumToStr(position.rot.q1,3) + " "; 70 | data := data + NumToStr(position.rot.q2,3) + " "; 71 | data := data + NumToStr(position.rot.q3,3) + " "; 72 | data := data + NumToStr(position.rot.q4,3); !End of string 73 | IF connected = TRUE THEN 74 | SocketSend clientSocket \Str:=data; 75 | ENDIF 76 | WaitTime loggerWaitTime; 77 | 78 | !Joint Coordinates 79 | joints := CJointT(); 80 | data := "# 1 "; 81 | data := data + date + " " + time + " "; 82 | data := data + NumToStr(ClkRead(timer),2) + " "; 83 | data := data + NumToStr(joints.robax.rax_1,2) + " "; 84 | data := data + NumToStr(joints.robax.rax_2,2) + " "; 85 | data := data + NumToStr(joints.robax.rax_3,2) + " "; 86 | data := data + NumToStr(joints.robax.rax_4,2) + " "; 87 | data := data + NumToStr(joints.robax.rax_5,2) + " "; 88 | data := data + NumToStr(joints.robax.rax_6,2); !End of string 89 | IF connected = TRUE THEN 90 | SocketSend clientSocket \Str:=data; 91 | ENDIF 92 | WaitTime loggerWaitTime; 93 | ENDWHILE 94 | ERROR 95 | IF ERRNO=ERR_SOCK_CLOSED THEN 96 | TPWrite "LOGGER: Client has closed connection."; 97 | ELSE 98 | TPWrite "LOGGER: Connection lost: Unknown problem."; 99 | ENDIF 100 | connected:=FALSE; 101 | !Closing the server 102 | SocketClose clientSocket; 103 | SocketClose serverSocket; 104 | !Reinitiate the server 105 | ServerCreateAndConnect ipController,loggerPort; 106 | connected:= TRUE; 107 | RETRY; 108 | ENDPROC 109 | 110 | ENDMODULE -------------------------------------------------------------------------------- /RAPID/SERVER.mod: -------------------------------------------------------------------------------- 1 | MODULE SERVER 2 | 3 | !//////////////// 4 | !GLOBAL VARIABLES 5 | !//////////////// 6 | 7 | !//Robot configuration 8 | PERS tooldata currentTool := [TRUE,[[0,0,0],[1,0,0,0]],[0.001,[0,0,0.001],[1,0,0,0],0,0,0]]; 9 | PERS wobjdata currentWobj := [FALSE,TRUE,"",[[0,0,0],[1,0,0,0]],[[0,0,0],[1,0,0,0]]]; 10 | PERS speeddata currentSpeed; 11 | PERS zonedata currentZone; 12 | 13 | !// Clock Synchronization 14 | PERS bool startLog:=TRUE; 15 | PERS bool startRob:=TRUE; 16 | 17 | !// Mutex between logger and changing the tool and work objects 18 | PERS bool frameMutex:=FALSE; 19 | 20 | !//PC communication 21 | VAR socketdev clientSocket; 22 | VAR socketdev serverSocket; 23 | VAR num instructionCode; 24 | VAR num params{10}; 25 | VAR num nParams; 26 | 27 | !PERS string ipController:= "192.168.125.1"; !robot default IP 28 | PERS string ipController:= "127.0.0.1"; !local IP for testing in simulation 29 | PERS num serverPort:= 5000; 30 | 31 | !//Motion of the robot 32 | VAR robtarget cartesianTarget; 33 | VAR jointtarget jointsTarget; 34 | VAR bool moveCompleted; !Set to true after finishing a Move instruction. 35 | 36 | !//Buffered move variables 37 | CONST num MAX_BUFFER := 512; 38 | VAR num BUFFER_POS := 0; 39 | VAR robtarget bufferTargets{MAX_BUFFER}; 40 | VAR speeddata bufferSpeeds{MAX_BUFFER}; 41 | 42 | !//External axis position variables 43 | VAR extjoint externalAxis; 44 | 45 | !//Circular move buffer 46 | VAR robtarget circPoint; 47 | 48 | !//Correct Instruction Execution and possible return values 49 | VAR num ok; 50 | CONST num SERVER_BAD_MSG := 0; 51 | CONST num SERVER_OK := 1; 52 | 53 | 54 | 55 | 56 | !//////////////// 57 | !LOCAL METHODS 58 | !//////////////// 59 | 60 | !//Method to parse the message received from a PC 61 | !// If correct message, loads values on: 62 | !// - instructionCode. 63 | !// - nParams: Number of received parameters. 64 | !// - params{nParams}: Vector of received params. 65 | PROC ParseMsg(string msg) 66 | !//Local variables 67 | VAR bool auxOk; 68 | VAR num ind:=1; 69 | VAR num newInd; 70 | VAR num length; 71 | VAR num indParam:=1; 72 | VAR string subString; 73 | VAR bool end := FALSE; 74 | 75 | !//Find the end character 76 | length := StrMatch(msg,1,"#"); 77 | IF length > StrLen(msg) THEN 78 | !//Corrupt message 79 | nParams := -1; 80 | ELSE 81 | !//Read Instruction code 82 | newInd := StrMatch(msg,ind," ") + 1; 83 | subString := StrPart(msg,ind,newInd - ind - 1); 84 | auxOk:= StrToVal(subString, instructionCode); 85 | IF auxOk = FALSE THEN 86 | !//Impossible to read instruction code 87 | nParams := -1; 88 | ELSE 89 | ind := newInd; 90 | !//Read all instruction parameters (maximum of 8) 91 | WHILE end = FALSE DO 92 | newInd := StrMatch(msg,ind," ") + 1; 93 | IF newInd > length THEN 94 | end := TRUE; 95 | ELSE 96 | subString := StrPart(msg,ind,newInd - ind - 1); 97 | auxOk := StrToVal(subString, params{indParam}); 98 | indParam := indParam + 1; 99 | ind := newInd; 100 | ENDIF 101 | ENDWHILE 102 | nParams:= indParam - 1; 103 | ENDIF 104 | ENDIF 105 | ENDPROC 106 | 107 | 108 | !//Handshake between server and client: 109 | !// - Creates socket. 110 | !// - Waits for incoming TCP connection. 111 | PROC ServerCreateAndConnect(string ip, num port) 112 | VAR string clientIP; 113 | 114 | SocketCreate serverSocket; 115 | SocketBind serverSocket, ip, port; 116 | SocketListen serverSocket; 117 | TPWrite "SERVER: Server waiting for incoming connections ..."; 118 | WHILE SocketGetStatus(clientSocket) <> SOCKET_CONNECTED DO 119 | SocketAccept serverSocket,clientSocket \ClientAddress:=clientIP \Time:=WAIT_MAX; 120 | IF SocketGetStatus(clientSocket) <> SOCKET_CONNECTED THEN 121 | TPWrite "SERVER: Problem serving an incoming connection."; 122 | TPWrite "SERVER: Try reconnecting."; 123 | ENDIF 124 | !//Wait 0.5 seconds for the next reconnection 125 | WaitTime 0.5; 126 | ENDWHILE 127 | TPWrite "SERVER: Connected to IP " + clientIP; 128 | ENDPROC 129 | 130 | 131 | !//Parameter initialization 132 | !// Loads default values for 133 | !// - Tool. 134 | !// - WorkObject. 135 | !// - Zone. 136 | !// - Speed. 137 | PROC Initialize() 138 | currentTool := [TRUE,[[0,0,0],[1,0,0,0]],[0.001,[0,0,0.001],[1,0,0,0],0,0,0]]; 139 | currentWobj := [FALSE,TRUE,"",[[0,0,0],[1,0,0,0]],[[0,0,0],[1,0,0,0]]]; 140 | currentSpeed := [100, 50, 0, 0]; 141 | currentZone := [FALSE, 0.3, 0.3,0.3,0.03,0.3,0.03]; !z0 142 | 143 | !Find the current external axis values so they don't move when we start 144 | jointsTarget := CJointT(); 145 | externalAxis := jointsTarget.extax; 146 | ENDPROC 147 | 148 | 149 | !//////////////////////// 150 | !//SERVER: Main procedure 151 | !//////////////////////// 152 | PROC main() 153 | !//Local variables 154 | VAR string receivedString; !//Received string 155 | VAR string sendString; !//Reply string 156 | VAR string addString; !//String to add to the reply. 157 | VAR bool connected; !//Client connected 158 | VAR bool reconnected; !//Drop and reconnection happened during serving a command 159 | VAR robtarget cartesianPose; 160 | VAR jointtarget jointsPose; 161 | 162 | !//Motion configuration 163 | ConfL \Off; 164 | SingArea \Wrist; 165 | moveCompleted:= TRUE; 166 | 167 | !//Initialization of WorkObject, Tool, Speed and Zone 168 | Initialize; 169 | 170 | !//Socket connection 171 | connected:=FALSE; 172 | ServerCreateAndConnect ipController,serverPort; 173 | connected:=TRUE; 174 | 175 | !//Server Loop 176 | WHILE TRUE DO 177 | !//Initialization of program flow variables 178 | ok:=SERVER_OK; !//Correctness of executed instruction. 179 | reconnected:=FALSE; !//Has communication dropped after receiving a command? 180 | addString := ""; 181 | 182 | !//Wait for a command 183 | SocketReceive clientSocket \Str:=receivedString \Time:=WAIT_MAX; 184 | ParseMsg receivedString; 185 | 186 | !//Execution of the command 187 | TEST instructionCode 188 | CASE 0: !Ping 189 | IF nParams = 0 THEN 190 | ok := SERVER_OK; 191 | ELSE 192 | ok := SERVER_BAD_MSG; 193 | ENDIF 194 | 195 | CASE 1: !Cartesian Move 196 | IF nParams = 7 THEN 197 | cartesianTarget :=[[params{1},params{2},params{3}], 198 | [params{4},params{5},params{6},params{7}], 199 | [0,0,0,0], 200 | externalAxis]; 201 | ok := SERVER_OK; 202 | moveCompleted := FALSE; 203 | MoveL cartesianTarget, currentSpeed, currentZone, currentTool \WObj:=currentWobj ; 204 | moveCompleted := TRUE; 205 | ELSE 206 | ok := SERVER_BAD_MSG; 207 | ENDIF 208 | 209 | CASE 2: !Joint Move 210 | IF nParams = 6 THEN 211 | jointsTarget:=[[params{1},params{2},params{3},params{4},params{5},params{6}], externalAxis]; 212 | ok := SERVER_OK; 213 | moveCompleted := FALSE; 214 | MoveAbsJ jointsTarget, currentSpeed, currentZone, currentTool \Wobj:=currentWobj; 215 | moveCompleted := TRUE; 216 | ELSE 217 | ok :=SERVER_BAD_MSG; 218 | ENDIF 219 | 220 | CASE 3: !Get Cartesian Coordinates (with current tool and workobject) 221 | IF nParams = 0 THEN 222 | cartesianPose := CRobT(\Tool:=currentTool \WObj:=currentWObj); 223 | addString := NumToStr(cartesianPose.trans.x,2) + " "; 224 | addString := addString + NumToStr(cartesianPose.trans.y,2) + " "; 225 | addString := addString + NumToStr(cartesianPose.trans.z,2) + " "; 226 | addString := addString + NumToStr(cartesianPose.rot.q1,3) + " "; 227 | addString := addString + NumToStr(cartesianPose.rot.q2,3) + " "; 228 | addString := addString + NumToStr(cartesianPose.rot.q3,3) + " "; 229 | addString := addString + NumToStr(cartesianPose.rot.q4,3); !End of string 230 | ok := SERVER_OK; 231 | ELSE 232 | ok :=SERVER_BAD_MSG; 233 | ENDIF 234 | 235 | CASE 4: !Get Joint Coordinates 236 | IF nParams = 0 THEN 237 | jointsPose := CJointT(); 238 | addString := NumToStr(jointsPose.robax.rax_1,2) + " "; 239 | addString := addString + NumToStr(jointsPose.robax.rax_2,2) + " "; 240 | addString := addString + NumToStr(jointsPose.robax.rax_3,2) + " "; 241 | addString := addString + NumToStr(jointsPose.robax.rax_4,2) + " "; 242 | addString := addString + NumToStr(jointsPose.robax.rax_5,2) + " "; 243 | addString := addString + NumToStr(jointsPose.robax.rax_6,2); !End of string 244 | ok := SERVER_OK; 245 | ELSE 246 | ok:=SERVER_BAD_MSG; 247 | ENDIF 248 | CASE 5: !Get external axis positions 249 | IF nParams = 0 THEN 250 | jointsPose := CJointT(); 251 | addString := StrPart(NumToStr(jointsTarget.extax.eax_a, 2),1,8) + " "; 252 | addString := addString + StrPart(NumToStr(jointsTarget.extax.eax_b,2),1,8) + " "; 253 | addString := addString + StrPart(NumToStr(jointsTarget.extax.eax_c,2),1,8) + " "; 254 | addString := addString + StrPart(NumToStr(jointsTarget.extax.eax_d,2),1,8) + " "; 255 | addString := addString + StrPart(NumToStr(jointsTarget.extax.eax_e,2),1,8) + " "; 256 | addString := addString + StrPart(NumToStr(jointsTarget.extax.eax_f,2),1,8); !End of string 257 | ok := SERVER_OK; 258 | ELSE 259 | ok:=SERVER_BAD_MSG; 260 | ENDIF 261 | 262 | CASE 6: !Set Tool 263 | IF nParams = 7 THEN 264 | WHILE (frameMutex) DO 265 | WaitTime .01; !// If the frame is being used by logger, wait here 266 | ENDWHILE 267 | frameMutex:= TRUE; 268 | currentTool.tframe.trans.x:=params{1}; 269 | currentTool.tframe.trans.y:=params{2}; 270 | currentTool.tframe.trans.z:=params{3}; 271 | currentTool.tframe.rot.q1:=params{4}; 272 | currentTool.tframe.rot.q2:=params{5}; 273 | currentTool.tframe.rot.q3:=params{6}; 274 | currentTool.tframe.rot.q4:=params{7}; 275 | ok := SERVER_OK; 276 | frameMutex:= FALSE; 277 | ELSE 278 | ok:=SERVER_BAD_MSG; 279 | ENDIF 280 | 281 | CASE 7: !Set Work Object 282 | IF nParams = 7 THEN 283 | currentWobj.oframe.trans.x:=params{1}; 284 | currentWobj.oframe.trans.y:=params{2}; 285 | currentWobj.oframe.trans.z:=params{3}; 286 | currentWobj.oframe.rot.q1:=params{4}; 287 | currentWobj.oframe.rot.q2:=params{5}; 288 | currentWobj.oframe.rot.q3:=params{6}; 289 | currentWobj.oframe.rot.q4:=params{7}; 290 | ok := SERVER_OK; 291 | ELSE 292 | ok:=SERVER_BAD_MSG; 293 | ENDIF 294 | 295 | CASE 8: !Set Speed of the Robot 296 | IF nParams = 4 THEN 297 | currentSpeed.v_tcp:=params{1}; 298 | currentSpeed.v_ori:=params{2}; 299 | currentSpeed.v_leax:=params{3}; 300 | currentSpeed.v_reax:=params{4}; 301 | ok := SERVER_OK; 302 | ELSEIF nParams = 2 THEN 303 | currentSpeed.v_tcp:=params{1}; 304 | currentSpeed.v_ori:=params{2}; 305 | ok := SERVER_OK; 306 | ELSE 307 | ok:=SERVER_BAD_MSG; 308 | ENDIF 309 | 310 | CASE 9: !Set zone data 311 | IF nParams = 4 THEN 312 | IF params{1}=1 THEN 313 | currentZone.finep := TRUE; 314 | currentZone.pzone_tcp := 0.0; 315 | currentZone.pzone_ori := 0.0; 316 | currentZone.zone_ori := 0.0; 317 | ELSE 318 | currentZone.finep := FALSE; 319 | currentZone.pzone_tcp := params{2}; 320 | currentZone.pzone_ori := params{3}; 321 | currentZone.zone_ori := params{4}; 322 | ENDIF 323 | ok := SERVER_OK; 324 | ELSE 325 | ok:=SERVER_BAD_MSG; 326 | ENDIF 327 | 328 | CASE 30: !Add Cartesian Coordinates to buffer 329 | IF nParams = 7 THEN 330 | cartesianTarget :=[[params{1},params{2},params{3}], 331 | [params{4},params{5},params{6},params{7}], 332 | [0,0,0,0], 333 | externalAxis]; 334 | IF BUFFER_POS < MAX_BUFFER THEN 335 | BUFFER_POS := BUFFER_POS + 1; 336 | bufferTargets{BUFFER_POS} := cartesianTarget; 337 | bufferSpeeds{BUFFER_POS} := currentSpeed; 338 | ENDIF 339 | ok := SERVER_OK; 340 | ELSE 341 | ok:=SERVER_BAD_MSG; 342 | ENDIF 343 | 344 | CASE 31: !Clear Cartesian Buffer 345 | IF nParams = 0 THEN 346 | BUFFER_POS := 0; 347 | ok := SERVER_OK; 348 | ELSE 349 | ok:=SERVER_BAD_MSG; 350 | ENDIF 351 | 352 | CASE 32: !Get Buffer Size) 353 | IF nParams = 0 THEN 354 | addString := NumToStr(BUFFER_POS,2); 355 | ok := SERVER_OK; 356 | ELSE 357 | ok:=SERVER_BAD_MSG; 358 | ENDIF 359 | 360 | CASE 33: !Execute moves in cartesianBuffer as linear moves 361 | IF nParams = 0 THEN 362 | FOR i FROM 1 TO (BUFFER_POS) DO 363 | MoveL bufferTargets{i}, bufferSpeeds{i}, currentZone, currentTool \WObj:=currentWobj ; 364 | ENDFOR 365 | ok := SERVER_OK; 366 | ELSE 367 | ok:=SERVER_BAD_MSG; 368 | ENDIF 369 | 370 | CASE 34: !External Axis move 371 | IF nParams = 6 THEN 372 | externalAxis :=[params{1},params{2},params{3},params{4},params{5},params{6}]; 373 | jointsTarget := CJointT(); 374 | jointsTarget.extax := externalAxis; 375 | ok := SERVER_OK; 376 | moveCompleted := FALSE; 377 | MoveAbsJ jointsTarget, currentSpeed, currentZone, currentTool \Wobj:=currentWobj; 378 | moveCompleted := TRUE; 379 | ELSE 380 | ok :=SERVER_BAD_MSG; 381 | ENDIF 382 | 383 | CASE 35: !Specify circPoint for circular move, and then wait on toPoint 384 | IF nParams = 7 THEN 385 | circPoint :=[[params{1},params{2},params{3}], 386 | [params{4},params{5},params{6},params{7}], 387 | [0,0,0,0], 388 | externalAxis]; 389 | ok := SERVER_OK; 390 | ELSE 391 | ok:=SERVER_BAD_MSG; 392 | ENDIF 393 | 394 | CASE 36: !specify toPoint, and use circPoint specified previously 395 | IF nParams = 7 THEN 396 | cartesianTarget :=[[params{1},params{2},params{3}], 397 | [params{4},params{5},params{6},params{7}], 398 | [0,0,0,0], 399 | externalAxis]; 400 | MoveC circPoint, cartesianTarget, currentSpeed, currentZone, currentTool \WObj:=currentWobj ; 401 | ok := SERVER_OK; 402 | ELSE 403 | ok:=SERVER_BAD_MSG; 404 | ENDIF 405 | 406 | CASE 98: !returns current robot info: serial number, robotware version, and robot type 407 | IF nParams = 0 THEN 408 | addString := GetSysInfo(\SerialNo) + "*"; 409 | addString := addString + GetSysInfo(\SWVersion) + "*"; 410 | addString := addString + GetSysInfo(\RobotType); 411 | ok := SERVER_OK; 412 | ELSE 413 | ok :=SERVER_BAD_MSG; 414 | ENDIF 415 | 416 | CASE 99: !Close Connection 417 | IF nParams = 0 THEN 418 | TPWrite "SERVER: Client has closed connection."; 419 | connected := FALSE; 420 | !//Closing the server 421 | SocketClose clientSocket; 422 | SocketClose serverSocket; 423 | 424 | !Reinitiate the server 425 | ServerCreateAndConnect ipController,serverPort; 426 | connected := TRUE; 427 | reconnected := TRUE; 428 | ok := SERVER_OK; 429 | ELSE 430 | ok := SERVER_BAD_MSG; 431 | ENDIF 432 | DEFAULT: 433 | TPWrite "SERVER: Illegal instruction code"; 434 | ok := SERVER_BAD_MSG; 435 | ENDTEST 436 | 437 | !Compose the acknowledge string to send back to the client 438 | IF connected = TRUE THEN 439 | IF reconnected = FALSE THEN 440 | IF SocketGetStatus(clientSocket) = SOCKET_CONNECTED THEN 441 | sendString := NumToStr(instructionCode,0); 442 | sendString := sendString + " " + NumToStr(ok,0); 443 | sendString := sendString + " " + addString; 444 | SocketSend clientSocket \Str:=sendString; 445 | ENDIF 446 | ENDIF 447 | ENDIF 448 | ENDWHILE 449 | 450 | ERROR (LONG_JMP_ALL_ERR) 451 | TPWrite "SERVER: ------"; 452 | TPWrite "SERVER: Error Handler:" + NumtoStr(ERRNO,0); 453 | TEST ERRNO 454 | CASE ERR_SOCK_CLOSED: 455 | TPWrite "SERVER: Lost connection to the client."; 456 | TPWrite "SERVER: Closing socket and restarting."; 457 | TPWrite "SERVER: ------"; 458 | connected:=FALSE; 459 | !//Closing the server 460 | SocketClose clientSocket; 461 | SocketClose serverSocket; 462 | !//Reinitiate the server 463 | ServerCreateAndConnect ipController,serverPort; 464 | reconnected:= FALSE; 465 | connected:= TRUE; 466 | RETRY; 467 | DEFAULT: 468 | TPWrite "SERVER: Unknown error."; 469 | TPWrite "SERVER: Closing socket and restarting."; 470 | TPWrite "SERVER: ------"; 471 | connected:=FALSE; 472 | !//Closing the server 473 | SocketClose clientSocket; 474 | SocketClose serverSocket; 475 | !//Reinitiate the server 476 | ServerCreateAndConnect ipController,serverPort; 477 | reconnected:= FALSE; 478 | connected:= TRUE; 479 | RETRY; 480 | ENDTEST 481 | ENDPROC 482 | 483 | ENDMODULE 484 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # open-abb-driver 2 | ### Control ABB robots remotely with ROS, Python, or C++ 3 | 4 | 5 | 6 | ## What is it? 7 | open-abb-driver consists of two main parts. The first is a program which is written in the ABB robot control language, RAPID, which allows remote clients to send requests for actions (such as joint moves, cartesian moves, speed changes, etc.). The second is a series of libraries to interact with the robot from remote computers, using several different control schemes. You can use the ROS driver, which allows control using ROS services and publishers. You can also include the Python or C++ libraries to communicate with the robot directly (both located in abb_node/packages/abb_comm), and bypass ROS completely. 8 | 9 | ## Requirements 10 | * ABB IRC5 controller 11 | * 6 DOF robotic manipulator 12 | * Robot must have the following factory software options 13 | * "PC Interface" 14 | * "Multitasking" (required for position feedback stream) 15 | 16 | ## Quick Start 17 | ### Robot Setup 18 | * Install the RAPID module 'SERVER' 19 | * Using RobotStudio online mode is the easiest way to do this, check out the [wiki article](https://github.com/robotics/open-abb-driver/wiki/Configuring-an-ABB-Robot-for-OAD) for details. 20 | * For position feedback, install the RAPID module 'LOGGER' into another task. 21 | * In SERVER.mod, check to make sure the "ipController" specified is the same as your robot. The default robot IP is 192.168.125.1 22 | * Start the programs on the robot 23 | * Production Window->PP to Main, then press the play button. 24 | 25 | ### Computer Setup 26 | * Verify that your computer is on the same subnet as the robot. 27 | * Try pinging the robot (default IP is 192.168.125.1). 28 | * Before trying ROS, it's pretty easy to check functionality using the [simple python interface.](https://github.com/robotics/open-abb-driver/wiki/Python-Control) 29 | * Note that you must either copy abb_node/packages/abb_comm/abb.py to your local directory or somewhere included in your PYTHONPATH environment. 30 | * To set up the ROS node (Fuerte only at the moment), copy abb_node to somewhere in your $ROS_PACKAGE_PATH. 31 | * If you did that correctly, try: 32 | 33 | ``` 34 | roscd abb_node 35 | rosmake abb_node 36 | roslaunch abb_node abb_tf.launch 37 | ``` 38 | -------------------------------------------------------------------------------- /abb_node/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | rosbuild_init() 5 | 6 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 7 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 8 | 9 | set(ABB_PATH ${PROJECT_SOURCE_DIR}/packages/abb_communications) 10 | set(MATVEC_PATH ${PROJECT_SOURCE_DIR}/packages/matVec) 11 | 12 | rosbuild_gensrv() 13 | 14 | rosbuild_add_library(matVec ${MATVEC_PATH}/HomogTransf.cpp 15 | ${MATVEC_PATH}/Mat.cpp 16 | ${MATVEC_PATH}/Polynom.cpp 17 | ${MATVEC_PATH}/Quaternion.cpp 18 | ${MATVEC_PATH}/RotMat.cpp 19 | ${MATVEC_PATH}/Vec.cpp) 20 | rosbuild_add_library(abb_comm ${ABB_PATH}/abb_comm.cpp) 21 | 22 | include_directories(${ABB_PATH}) 23 | include_directories(${MATVEC_PATH}) 24 | 25 | link_directories(${PROJECT_SOURCE_DIR}/lib) 26 | 27 | rosbuild_add_executable(abb_node src/abb_node.cpp) 28 | target_link_libraries(abb_node abb_comm matVec) 29 | -------------------------------------------------------------------------------- /abb_node/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /abb_node/launch/abb_tf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /abb_node/launch/robot_params.yaml: -------------------------------------------------------------------------------- 1 | robot: {speedORI: 10.0, speedTCP: 50.0, toolQ0: 1.0, 2 | toolQX: 0.0, toolQY: 0.0, toolQZ: 0.0, toolX: 0.0, toolY: 0.0, 3 | toolZ: 105.0, workobjectQ0: 0.7084, workobjectQX: 0.0003882, workobjectQY: -0.0003882, 4 | workobjectQZ: 0.7058, workobjectX: 808.5, workobjectY: -612.86, workobjectZ: 0.59, 5 | zone: 1, robotIp: 192.168.1.99, robotLoggerPort: 5001, robotMotionPort: 5000, vacuum: 0} 6 | 7 | -------------------------------------------------------------------------------- /abb_node/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | abb_node 4 | 5 | Alberto Rodriguez, Robbie Paolini, Michael Dawson-Haggerty 6 | BSD 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /abb_node/packages/abb_communications/abb.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Michael Dawson-Haggerty 3 | 4 | abb.py: contains classes and support functions which interact with an ABB Robot running our software stack (RAPID code module SERVER) 5 | 6 | 7 | For functions which require targets (XYZ positions with quaternion orientation), 8 | targets can be passed as [[XYZ], [Quats]] OR [XYZ, Quats] 9 | 10 | ''' 11 | 12 | import socket 13 | import json 14 | import time 15 | import inspect 16 | from threading import Thread 17 | from collections import deque 18 | import logging 19 | 20 | log = logging.getLogger(__name__) 21 | log.addHandler(logging.NullHandler()) 22 | 23 | class Robot: 24 | def __init__(self, 25 | ip = '192.168.125.1', 26 | port_motion = 5000, 27 | port_logger = 5001): 28 | 29 | self.delay = .08 30 | 31 | self.connect_motion((ip, port_motion)) 32 | #log_thread = Thread(target = self.get_net, 33 | # args = ((ip, port_logger))).start() 34 | 35 | self.set_units('millimeters', 'degrees') 36 | self.set_tool() 37 | self.set_workobject() 38 | self.set_speed() 39 | self.set_zone() 40 | 41 | def connect_motion(self, remote): 42 | log.info('Attempting to connect to robot motion server at %s', str(remote)) 43 | self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 44 | self.sock.settimeout(2.5) 45 | self.sock.connect(remote) 46 | self.sock.settimeout(None) 47 | log.info('Connected to robot motion server at %s', str(remote)) 48 | 49 | def connect_logger(self, remote, maxlen=None): 50 | self.pose = deque(maxlen=maxlen) 51 | self.joints = deque(maxlen=maxlen) 52 | 53 | s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 54 | s.connect(remote) 55 | s.setblocking(1) 56 | try: 57 | while True: 58 | data = map(float, s.recv(4096).split()) 59 | if int(data[1]) == 0: 60 | self.pose.append([data[2:5], data[5:]]) 61 | #elif int(data[1]) == 1: self.joints.append([a[2:5], a[5:]]) 62 | finally: 63 | s.shutdown(socket.SHUT_RDWR) 64 | 65 | def set_units(self, linear, angular): 66 | units_l = {'millimeters': 1.0, 67 | 'meters' : 1000.0, 68 | 'inches' : 25.4} 69 | units_a = {'degrees' : 1.0, 70 | 'radians' : 57.2957795} 71 | self.scale_linear = units_l[linear] 72 | self.scale_angle = units_a[angular] 73 | 74 | def set_cartesian(self, pose): 75 | ''' 76 | Executes a move immediately from the current pose, 77 | to 'pose', with units of millimeters. 78 | ''' 79 | msg = "01 " + self.format_pose(pose) 80 | return self.send(msg) 81 | 82 | def set_joints(self, joints): 83 | ''' 84 | Executes a move immediately, from current joint angles, 85 | to 'joints', in degrees. 86 | ''' 87 | if len(joints) != 6: return False 88 | msg = "02 " 89 | for joint in joints: msg += format(joint*self.scale_angle, "+08.2f") + " " 90 | msg += "#" 91 | return self.send(msg) 92 | 93 | def get_cartesian(self): 94 | ''' 95 | Returns the current pose of the robot, in millimeters 96 | ''' 97 | msg = "03 #" 98 | data = self.send(msg).split() 99 | r = [float(s) for s in data] 100 | return [r[2:5], r[5:9]] 101 | 102 | def get_joints(self): 103 | ''' 104 | Returns the current angles of the robots joints, in degrees. 105 | ''' 106 | msg = "04 #" 107 | data = self.send(msg).split() 108 | return [float(s) / self.scale_angle for s in data[2:8]] 109 | 110 | def get_external_axis(self): 111 | ''' 112 | If you have an external axis connected to your robot controller 113 | (such as a FlexLifter 600, google it), this returns the joint angles 114 | ''' 115 | msg = "05 #" 116 | data = self.send(msg).split() 117 | return [float(s) for s in data[2:8]] 118 | 119 | def get_robotinfo(self): 120 | ''' 121 | Returns a robot- unique string, with things such as the 122 | robot's model number. 123 | Example output from and IRB 2400: 124 | ['24-53243', 'ROBOTWARE_5.12.1021.01', '2400/16 Type B'] 125 | ''' 126 | msg = "98 #" 127 | data = str(self.send(msg))[5:].split('*') 128 | log.debug('get_robotinfo result: %s', str(data)) 129 | return data 130 | 131 | def set_tool(self, tool=[[0,0,0], [1,0,0,0]]): 132 | ''' 133 | Sets the tool centerpoint (TCP) of the robot. 134 | When you command a cartesian move, 135 | it aligns the TCP frame with the requested frame. 136 | 137 | Offsets are from tool0, which is defined at the intersection of the 138 | tool flange center axis and the flange face. 139 | ''' 140 | msg = "06 " + self.format_pose(tool) 141 | self.send(msg) 142 | self.tool = tool 143 | 144 | def load_json_tool(self, file_obj): 145 | if file_obj.__class__.__name__ == 'str': 146 | file_obj = open(filename, 'rb'); 147 | tool = check_coordinates(json.load(file_obj)) 148 | self.set_tool(tool) 149 | 150 | def get_tool(self): 151 | log.debug('get_tool returning: %s', str(self.tool)) 152 | return self.tool 153 | 154 | def set_workobject(self, work_obj=[[0,0,0],[1,0,0,0]]): 155 | ''' 156 | The workobject is a local coordinate frame you can define on the robot, 157 | then subsequent cartesian moves will be in this coordinate frame. 158 | ''' 159 | msg = "07 " + self.format_pose(work_obj) 160 | self.send(msg) 161 | 162 | def set_speed(self, speed=[100,50,50,50]): 163 | ''' 164 | speed: [robot TCP linear speed (mm/s), TCP orientation speed (deg/s), 165 | external axis linear, external axis orientation] 166 | ''' 167 | 168 | if len(speed) != 4: return False 169 | msg = "08 " 170 | msg += format(speed[0], "+08.1f") + " " 171 | msg += format(speed[1], "+08.2f") + " " 172 | msg += format(speed[2], "+08.1f") + " " 173 | msg += format(speed[3], "+08.2f") + " #" 174 | self.send(msg) 175 | 176 | def set_zone(self, 177 | zone_key = 'z1', 178 | point_motion = False, 179 | manual_zone = []): 180 | zone_dict = {'z0' : [.3,.3,.03], 181 | 'z1' : [1,1,.1], 182 | 'z5' : [5,8,.8], 183 | 'z10' : [10,15,1.5], 184 | 'z15' : [15,23,2.3], 185 | 'z20' : [20,30,3], 186 | 'z30' : [30,45,4.5], 187 | 'z50' : [50,75,7.5], 188 | 'z100': [100,150,15], 189 | 'z200': [200,300,30]} 190 | ''' 191 | Sets the motion zone of the robot. This can also be thought of as 192 | the flyby zone, AKA if the robot is going from point A -> B -> C, 193 | how close do we have to pass by B to get to C 194 | 195 | zone_key: uses values from RAPID handbook (stored here in zone_dict) 196 | with keys 'z*', you should probably use these 197 | 198 | point_motion: go to point exactly, and stop briefly before moving on 199 | 200 | manual_zone = [pzone_tcp, pzone_ori, zone_ori] 201 | pzone_tcp: mm, radius from goal where robot tool centerpoint 202 | is not rigidly constrained 203 | pzone_ori: mm, radius from goal where robot tool orientation 204 | is not rigidly constrained 205 | zone_ori: degrees, zone size for the tool reorientation 206 | ''' 207 | 208 | if point_motion: 209 | zone = [0,0,0] 210 | elif len(manual_zone) == 3: 211 | zone = manual_zone 212 | elif zone_key in zone_dict.keys(): 213 | zone = zone_dict[zone_key] 214 | else: return False 215 | 216 | msg = "09 " 217 | msg += str(int(point_motion)) + " " 218 | msg += format(zone[0], "+08.4f") + " " 219 | msg += format(zone[1], "+08.4f") + " " 220 | msg += format(zone[2], "+08.4f") + " #" 221 | self.send(msg) 222 | 223 | def buffer_add(self, pose): 224 | ''' 225 | Appends single pose to the remote buffer 226 | Move will execute at current speed (which you can change between buffer_add calls) 227 | ''' 228 | msg = "30 " + self.format_pose(pose) 229 | self.send(msg) 230 | 231 | def buffer_set(self, pose_list): 232 | ''' 233 | Adds every pose in pose_list to the remote buffer 234 | ''' 235 | self.clear_buffer() 236 | for pose in pose_list: 237 | self.buffer_add(pose) 238 | if self.buffer_len() == len(pose_list): 239 | log.debug('Successfully added %i poses to remote buffer', 240 | len(pose_list)) 241 | return True 242 | else: 243 | log.warn('Failed to add poses to remote buffer!') 244 | self.clear_buffer() 245 | return False 246 | 247 | def clear_buffer(self): 248 | msg = "31 #" 249 | data = self.send(msg) 250 | if self.buffer_len() != 0: 251 | log.warn('clear_buffer failed! buffer_len: %i', self.buffer_len()) 252 | raise NameError('clear_buffer failed!') 253 | return data 254 | 255 | def buffer_len(self): 256 | ''' 257 | Returns the length (number of poses stored) of the remote buffer 258 | ''' 259 | msg = "32 #" 260 | data = self.send(msg).split() 261 | return int(float(data[2])) 262 | 263 | def buffer_execute(self): 264 | ''' 265 | Immediately execute linear moves to every pose in the remote buffer. 266 | ''' 267 | msg = "33 #" 268 | return self.send(msg) 269 | 270 | def set_external_axis(self, axis_unscaled=[-550,0,0,0,0,0]): 271 | if len(axis_values) != 6: return False 272 | msg = "34 " 273 | for axis in axis_values: 274 | msg += format(axis, "+08.2f") + " " 275 | msg += "#" 276 | return self.send(msg) 277 | 278 | def move_circular(self, pose_onarc, pose_end): 279 | ''' 280 | Executes a movement in a circular path from current position, 281 | through pose_onarc, to pose_end 282 | ''' 283 | msg_0 = "35 " + self.format_pose(pose_onarc) 284 | msg_1 = "36 " + self.format_pose(pose_end) 285 | 286 | data = self.send(msg_0).split() 287 | if data[1] != '1': 288 | log.warn('move_circular incorrect response, bailing!') 289 | return False 290 | return self.send(msg_1) 291 | 292 | def set_dio(self, value, id=0): 293 | ''' 294 | A function to set a physical DIO line on the robot. 295 | For this to work you're going to need to edit the RAPID function 296 | and fill in the DIO you want this to switch. 297 | ''' 298 | msg = '97 ' + str(int(bool(value))) + ' #' 299 | return 300 | #return self.send(msg) 301 | 302 | def send(self, message, wait_for_response=True): 303 | ''' 304 | Send a formatted message to the robot socket. 305 | if wait_for_response, we wait for the response and return it 306 | ''' 307 | caller = inspect.stack()[1][3] 308 | log.debug('%-14s sending: %s', caller, message) 309 | self.sock.send(message) 310 | time.sleep(self.delay) 311 | if not wait_for_response: return 312 | data = self.sock.recv(4096) 313 | log.debug('%-14s recieved: %s', caller, data) 314 | return data 315 | 316 | def format_pose(self, pose): 317 | pose = check_coordinates(pose) 318 | msg = '' 319 | for cartesian in pose[0]: 320 | msg += format(cartesian * self.scale_linear, "+08.1f") + " " 321 | for quaternion in pose[1]: 322 | msg += format(quaternion, "+08.5f") + " " 323 | msg += "#" 324 | return msg 325 | 326 | def close(self): 327 | self.send("99 #", False) 328 | self.sock.shutdown(socket.SHUT_RDWR) 329 | self.sock.close() 330 | log.info('Disconnected from ABB robot.') 331 | 332 | def __enter__(self): 333 | return self 334 | 335 | def __exit__(self, type, value, traceback): 336 | self.close() 337 | 338 | def check_coordinates(coordinates): 339 | if ((len(coordinates) == 2) and 340 | (len(coordinates[0]) == 3) and 341 | (len(coordinates[1]) == 4)): 342 | return coordinates 343 | elif (len(coordinates) == 7): 344 | return [coordinates[0:3], coordinates[3:7]] 345 | log.warn('Recieved malformed coordinate: %s', str(coordinates)) 346 | raise NameError('Malformed coordinate!') 347 | 348 | if __name__ == '__main__': 349 | formatter = logging.Formatter("[%(asctime)s] %(levelname)-7s (%(filename)s:%(lineno)3s) %(message)s", "%Y-%m-%d %H:%M:%S") 350 | handler_stream = logging.StreamHandler() 351 | handler_stream.setFormatter(formatter) 352 | handler_stream.setLevel(logging.DEBUG) 353 | log = logging.getLogger('abb') 354 | log.setLevel(logging.DEBUG) 355 | log.addHandler(handler_stream) 356 | 357 | -------------------------------------------------------------------------------- /abb_node/packages/abb_communications/abb_comm.cpp: -------------------------------------------------------------------------------- 1 | #include "abb_comm.h" 2 | 3 | /** 4 | * Formats message to ping the ABB robot. 5 | * @param idCode User code identifying the message. Will be sent back with the acknowledgement. 6 | * @return String to be sent to ABB server. 7 | */ 8 | string abb_comm::pingRobot(int idCode) 9 | { 10 | char buff[10]; 11 | string msg("00 ");//instruction code; 12 | 13 | sprintf(buff,"%.3d ",idCode); //identification code 14 | msg += buff; 15 | msg += "#"; 16 | 17 | return (msg); 18 | } 19 | 20 | /** 21 | * Formats message to set the cartesian coordinates of the ABB robot. 22 | * The coordinates are always with respect to the currently defined work object and tool. 23 | * @param x X-coordinate of the robot. 24 | * @param y Y-coordinate of the robot. 25 | * @param z Z-coordinate of the robot. 26 | * @param q0 First component of the orientation quaternion. 27 | * @param qx Second component of the orientation quaternion. 28 | * @param qy Third component of the orientation quaternion. 29 | * @param qz Fourth component of the orientation quaternion. 30 | * @param idCode User code identifying the message. Will be sent back with the acknowledgement. 31 | * @return String to be sent to ABB server. 32 | */ 33 | string abb_comm::setCartesian(double x, double y, double z, double q0, double qx, double qy, double qz, int idCode) 34 | { 35 | char buff[10]; 36 | string msg("01 ");//instruction code; 37 | 38 | sprintf(buff,"%.3d ",idCode); //identification code 39 | msg += buff; 40 | sprintf(buff,"%+08.1lf ",x); 41 | msg += buff ; 42 | sprintf(buff,"%+08.1lf ",y); 43 | msg += buff ; 44 | sprintf(buff,"%+08.1lf ",z); 45 | msg += buff ; 46 | sprintf(buff,"%+08.5lf ",q0); 47 | msg += buff ; 48 | sprintf(buff,"%+08.5lf ",qx); 49 | msg += buff ; 50 | sprintf(buff,"%+08.5lf ",qy); 51 | msg += buff ; 52 | sprintf(buff,"%+08.5lf ",qz); 53 | msg += buff ; 54 | msg += "#"; 55 | 56 | return (msg); 57 | } 58 | 59 | /** 60 | * Formats message to set the joint coordinates of the ABB robot. 61 | * @param joint1 Value of joint 1. 62 | * @param joint2 Value of joint 2. 63 | * @param joint3 Value of joint 3. 64 | * @param joint4 Value of joint 4. 65 | * @param joint5 Value of joint 5. 66 | * @param joint6 Value of joint 6. 67 | * @param idCode User code identifying the message. Will be sent back with the acknowledgement. 68 | * @return String to be sent to ABB server. 69 | */ 70 | string abb_comm::setJoints(double joint1, double joint2, double joint3, double joint4, double joint5, double joint6, int idCode) 71 | { 72 | char buff[10]; 73 | string msg("02 ");//instruction code; 74 | 75 | sprintf(buff,"%.3d ",idCode); //identification code 76 | msg += buff; 77 | sprintf(buff,"%+08.2lf ",joint1); 78 | msg += buff ; 79 | sprintf(buff,"%+08.2lf ",joint2); 80 | msg += buff ; 81 | sprintf(buff,"%+08.2lf ",joint3); 82 | msg += buff ; 83 | sprintf(buff,"%+08.2lf ",joint4); 84 | msg += buff ; 85 | sprintf(buff,"%+08.2lf ",joint5); 86 | msg += buff ; 87 | sprintf(buff,"%+08.2lf ",joint6); 88 | msg += buff ; 89 | msg += "#"; 90 | 91 | return (msg); 92 | } 93 | 94 | /** 95 | * Formats message to query the ABB robot for its cartesian coordinates. 96 | * @param idCode User code identifying the message. Will be sent back with the acknowledgement. 97 | * @return String to be sent to ABB server. 98 | */ 99 | string abb_comm::getCartesian(int idCode) 100 | { 101 | char buff[10]; 102 | string msg("03 ");//instruction code; 103 | 104 | sprintf(buff,"%.3d ",idCode); //identification code 105 | msg += buff; 106 | msg += "#"; 107 | return (msg); 108 | } 109 | 110 | /** 111 | * Formats message to query the ABB robot for its joint axis coordinates. 112 | * @param idCode User code identifying the message. Will be sent back with the acknowledgement. 113 | * @return String to be sent to ABB server. 114 | */ 115 | string abb_comm::getJoints(int idCode) 116 | { 117 | char buff[10]; 118 | string msg("04 ");//instruction code; 119 | 120 | sprintf(buff,"%.3d ",idCode); //identification code 121 | msg += buff; 122 | msg += "#"; 123 | return (msg); 124 | } 125 | 126 | /** 127 | * Formats message to define the tool coordinates. 128 | * @param x X-coordinate of the tool. 129 | * @param y Y-coordinate of the tool. 130 | * @param z Z-coordinate of the tool. 131 | * @param q0 First component of the orientation quaternion of the tool. 132 | * @param qx Second component of the orientation quaternion of the tool. 133 | * @param qy Third component of the orientation quaternion of the tool. 134 | * @param qz Fourth component of the orientation quaternion of the tool. 135 | * @param idCode User code identifying the message. Will be sent back with the acknowledgement. 136 | * @return String to be sent to ABB server. 137 | */ 138 | string abb_comm::setTool(double x, double y, double z, double q0, double qx, double qy, double qz, int idCode) 139 | { 140 | char buff[10]; 141 | string msg("06 ");//instruction code; 142 | 143 | sprintf(buff,"%.3d ",idCode); //identification code 144 | msg += buff; 145 | sprintf(buff,"%+08.1lf ",x); 146 | msg += buff ; 147 | sprintf(buff,"%+08.1lf ",y); 148 | msg += buff ; 149 | sprintf(buff,"%+08.1lf ",z); 150 | msg += buff ; 151 | sprintf(buff,"%+08.5lf ",q0); 152 | msg += buff ; 153 | sprintf(buff,"%+08.5lf ",qx); 154 | msg += buff ; 155 | sprintf(buff,"%+08.5lf ",qy); 156 | msg += buff ; 157 | sprintf(buff,"%+08.5lf ",qz); 158 | msg += buff ; 159 | msg += "#"; 160 | 161 | return (msg); 162 | } 163 | 164 | /** 165 | * Formats message to define the coordinates of the work object reference frame. 166 | * @param x X-coordinate of the work object reference frame. 167 | * @param y Y-coordinate of the work object reference frame. 168 | * @param z Z-coordinate of the work object reference frame. 169 | * @param q0 First component of the orientation quaternion of the work object reference frame. 170 | * @param qx Second component of the orientation quaternion of the work object reference frame. 171 | * @param qy Third component of the orientation quaternion of the work object reference frame. 172 | * @param qz Fourth component of the orientation quaternion of the work object reference frame. 173 | * @param idCode User code identifying the message. Will be sent back with the acknowledgement. 174 | * @return String to be sent to ABB server. 175 | */ 176 | string abb_comm::setWorkObject(double x, double y, double z, double q0, double qx, double qy, double qz, int idCode) 177 | { 178 | char buff[10]; 179 | string msg("07 ");//instruction code; 180 | 181 | sprintf(buff,"%.3d ",idCode); //identification code 182 | msg += buff; 183 | sprintf(buff,"%+08.1lf ",x); 184 | msg += buff ; 185 | sprintf(buff,"%+08.1lf ",y); 186 | msg += buff ; 187 | sprintf(buff,"%+08.1lf ",z); 188 | msg += buff ; 189 | sprintf(buff,"%+08.5lf ",q0); 190 | msg += buff ; 191 | sprintf(buff,"%+08.5lf ",qx); 192 | msg += buff ; 193 | sprintf(buff,"%+08.5lf ",qy); 194 | msg += buff ; 195 | sprintf(buff,"%+08.5lf ",qz); 196 | msg += buff ; 197 | msg += "#"; 198 | 199 | return (msg); 200 | } 201 | 202 | /** 203 | * Formats message to set the speed of the ABB robot. 204 | * The values specified for tcp and ori are modified by the percentage of override specified by the operator in the teach pendant. 205 | * @param tcp Linear speed of the TCP in mm/s (max recommended value ~ 500). 206 | * @param ori Reorientation speed of the TCP in deg/s (max recommended value ~ 150). 207 | * @param idCode User code identifying the message. Will be sent back with the acknowledgement. 208 | * @return String to be sent to ABB server. 209 | */ 210 | string abb_comm::setSpeed(double tcp, double ori, int idCode) 211 | { 212 | char buff[10]; 213 | string msg("08 ");//instruction code; 214 | 215 | sprintf(buff,"%.3d ",idCode); //identification code 216 | msg += buff; 217 | sprintf(buff,"%08.1lf ",tcp); 218 | msg += buff ; 219 | sprintf(buff,"%08.2lf ",ori); 220 | msg += buff ; 221 | msg += "#"; 222 | 223 | return (msg); 224 | } 225 | 226 | 227 | string abb_comm::setZone(bool fine, double tcp_mm, double ori_mm, double ori_deg, int idCode) 228 | { 229 | char buff[10]; 230 | string msg("09 ");//instruction code; 231 | 232 | sprintf(buff,"%.3d ",idCode); //identification code 233 | msg += buff; 234 | sprintf(buff,"%.1d ",fine); 235 | msg += buff ; 236 | sprintf(buff,"%.2lf ", tcp_mm); 237 | msg += buff ; 238 | sprintf(buff,"%.2lf ", ori_mm); 239 | msg += buff ; 240 | sprintf(buff,"%.2lf ", ori_deg); 241 | msg += buff ; 242 | msg += "#"; 243 | 244 | return (msg); 245 | } 246 | 247 | /** 248 | * Formats message to call special command. 249 | * @param command Number identifying the special command. 250 | * @param param1 General purpose parameter 1. 251 | * @param param2 General purpose parameter 2. 252 | * @param param3 General purpose parameter 3. 253 | * @param param4 General purpose parameter 4. 254 | * @param param5 General purpose parameter 5. 255 | * @param idCode User code identifying the message. Will be sent back with the acknowledgement. 256 | * @return String to be sent to ABB server. 257 | */ 258 | string abb_comm::specialCommand(int command, double param1, double param2, double param3, double param4, double param5, int idCode) 259 | { 260 | char buff[12]; 261 | string msg("10 ");//instruction code; 262 | 263 | sprintf(buff,"%.3d ",idCode); //identification code 264 | msg += buff; 265 | sprintf(buff,"%.1d ",command); 266 | msg += buff ; 267 | sprintf(buff,"%+09.2lf ", param1); 268 | msg += buff ; 269 | sprintf(buff,"%+09.2lf ", param2); 270 | msg += buff ; 271 | sprintf(buff,"%+09.2lf ", param3); 272 | msg += buff ; 273 | sprintf(buff,"%+09.2lf ", param4); 274 | msg += buff ; 275 | sprintf(buff,"%+09.2lf ", param5); 276 | msg += buff ; 277 | msg += "#"; 278 | 279 | return (msg); 280 | } 281 | 282 | /** 283 | * Formats message to set the vacuum on/off. 284 | * @param vacuum 1-on 0-off. 285 | * @param idCode User code identifying the message. Will be sent back with the acknowledgement. 286 | * @return String to be sent to ABB server. 287 | */ 288 | string abb_comm::setVacuum(int vacuum, int idCode) 289 | { 290 | char buff[10]; 291 | string msg("11 ");//instruction code; 292 | 293 | sprintf(buff,"%.3d ",idCode); //identification code 294 | msg += buff; 295 | sprintf(buff,"%.2d ",vacuum); 296 | msg += buff ; 297 | msg += "#"; 298 | 299 | return (msg); 300 | } 301 | 302 | 303 | 304 | string abb_comm::setDIO(int dio_number, int dio_state, int idCode) 305 | { 306 | char buff[10]; 307 | string msg("26 ");//instruction code; 308 | 309 | sprintf(buff,"%.3d ",idCode); //identification code 310 | msg += buff; 311 | sprintf(buff,"%.2d ", dio_number); 312 | msg += buff ; 313 | sprintf(buff,"%.2d ", dio_state); 314 | msg += buff ; 315 | msg += "#"; 316 | return (msg); 317 | } 318 | 319 | 320 | /** 321 | * Formats message to close the connection with the server in the ABB robot. 322 | * @param idCode User code identifying the message. Will be sent back with the acknowledgement. 323 | * @return String to be sent to ABB server. 324 | */ 325 | string abb_comm::closeConnection(int idCode) 326 | { 327 | char buff[10]; 328 | string msg("99 ");//instruction code; 329 | 330 | sprintf(buff,"%.3d ",idCode); //identification code 331 | msg += buff; 332 | msg += "#"; 333 | 334 | return (msg); 335 | } 336 | 337 | /** 338 | * Parser for the answer from the controller to the command getCartesian(). 339 | * @param msg String message to parse. 340 | * @param x Placer for the X-coordinate of the ABB robot. 341 | * @param y Placer for the Y-coordinate of the ABB robot. 342 | * @param z Placer for the Z-coordinate of the ABB robot. 343 | * @param q0 Placer for the first component of the orientation quaternion of the ABB robot. 344 | * @param qx Placer for the second component of the orientation quaternion of the ABB robot. 345 | * @param qy Placer for the third component of the orientation quaternion of the ABB robot. 346 | * @param qz Placer for the fourth component of the orientation quaternion of the ABB robot. 347 | * @return Whether the message was received correctly or not by the ABB robot. 348 | */ 349 | int abb_comm::parseCartesian(std::string msg, double *x, double *y, 350 | double *z,double *q0, double *qx, double *qy, double*qz) 351 | { 352 | int ok, idCode; 353 | sscanf(msg.c_str(),"%*d %d %d %*f %lf %lf %lf %lf %lf %lf %lf",&idCode,&ok,x,y,z,q0,qx,qy,qz); 354 | if (ok) 355 | return idCode; 356 | else 357 | return -1; 358 | } 359 | 360 | /** 361 | * Parser for the answer from the controller to the command getJoints(). 362 | * @param msg String message to parse. 363 | * @param joint1 Placer for the joint 1 of the ABB robot. 364 | * @param joint2 Placer for the joint 2 of the ABB robot. 365 | * @param joint3 Placer for the joint 3 of the ABB robot. 366 | * @param joint4 Placer for the joint 4 of the ABB robot. 367 | * @param joint5 Placer for the joint 5 of the ABB robot. 368 | * @param joint6 Placer for the joint 6 of the ABB robot. 369 | * @return Whether the message was received correctly or not by the ABB robot. 370 | */ 371 | int abb_comm::parseJoints(std::string msg, double *joint1, 372 | double *joint2, double *joint3, double *joint4, 373 | double *joint5, double *joint6) 374 | { 375 | int ok, idCode; 376 | sscanf(msg.c_str(),"%*d %d %d %*f %lf %lf %lf %lf %lf %lf",&idCode,&ok,joint1,joint2,joint3,joint4,joint5,joint6); 377 | if (ok) 378 | return idCode; 379 | else 380 | return -1; 381 | } 382 | -------------------------------------------------------------------------------- /abb_node/packages/abb_communications/abb_comm.h: -------------------------------------------------------------------------------- 1 | #ifndef ABBINTERPRETER_H 2 | #define ABBINTERPRETER_H 3 | 4 | #include 5 | #include 6 | #include "math.h" 7 | #include 8 | 9 | using namespace std; 10 | 11 | /** \class namespace 12 | \brief ABB server interpreter. 13 | Collection of methods to format and parse messages between PC and server running in ABB controller. 14 | */ 15 | namespace abb_comm 16 | { 17 | string pingRobot(int idCode=0); 18 | string setCartesian(double x, double y, double z, double q0, double qx, double qy, double qz, int idCode=0); 19 | string setJoints(double joint1, double joint2, double joint3, double joint4, double joint5, double joint6, int idCode=0); 20 | string getCartesian(int idCode=0); 21 | string getJoints(int idCode=0); 22 | string setTool(double x, double y, double z, double q0, double qx, double qy, double qz, int idCode=0); 23 | string setWorkObject(double x, double y, double z, double q0, double qx, double qy, double qz, int idCode=0); 24 | string setSpeed(double tcp, double ori, int idCode=0); 25 | string setZone(bool fine=0, double tcp_mm = 5.0, double ori_mm = 5.0, double ori_deg = 1.0, int idCode=0); 26 | string specialCommand(int command, double param1, double param2, double param3, double param4, double param5, int idCode=0); 27 | string setVacuum(int vacuum=0, int idCode=0); 28 | string setDIO(int dio_number=0, int dio_state=0, int idCode=0); 29 | string closeConnection(int idCode=0); 30 | int parseCartesian(string msg, double *x, double *y, double *z, 31 | double *q0, double *qx, double *qy, double *qz); 32 | int parseJoints(string msg, double *joint1, double *joint2, 33 | double *joint3, double *joint4, double *joint5, double *joint6); 34 | } 35 | #endif 36 | -------------------------------------------------------------------------------- /abb_node/packages/abb_communications/tool.object.json: -------------------------------------------------------------------------------- 1 | [[0, 0, 0], [1, 0, 0, 0]] -------------------------------------------------------------------------------- /abb_node/packages/matVec/HomogTransf.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "Vec.h" 4 | #include "Mat.h" 5 | #include "RotMat.h" 6 | #include "HomogTransf.h" 7 | 8 | HomogTransf::HomogTransf() : Mat(0.0,4,4) 9 | { 10 | for(int i=0;i<4;i++) v[i][i]=1.0; 11 | } 12 | 13 | HomogTransf::HomogTransf(const HomogTransf &original) : Mat(original){} 14 | 15 | HomogTransf::HomogTransf(const double *values) : Mat(values,4,4){} 16 | 17 | HomogTransf::HomogTransf(const char *string) : Mat(string,4,4){} 18 | 19 | HomogTransf::HomogTransf(const RotMat &rot, const Vec &trans) : Mat(4,4) 20 | { 21 | this->setRotation(rot); 22 | this->setTranslation(trans); 23 | v[3][0]=0; 24 | v[3][1]=0; 25 | v[3][2]=0; 26 | v[3][3]=1; 27 | } 28 | 29 | HomogTransf::HomogTransf(const Mat &original) : Mat(4,4) 30 | { 31 | if ( (original.nn==4) && (original.mm==4) ) 32 | { 33 | for(int i=0;i<4;i++) { 34 | for(int j=0;j<4;j++) { 35 | v[i][j]=original[i][j]; 36 | } 37 | } 38 | } 39 | } 40 | 41 | HomogTransf & HomogTransf::operator=(const double constant){return(*this=HomogTransf((Mat)*this=constant));} 42 | HomogTransf & HomogTransf::operator+=(const HomogTransf &original){return(*this=HomogTransf((Mat)*this+=(Mat)original));} 43 | HomogTransf & HomogTransf::operator-=(const HomogTransf &original){return(*this=HomogTransf((Mat)*this-=(Mat)original));} 44 | HomogTransf & HomogTransf::operator*=(const double constant){return(*this=HomogTransf((Mat)*this*=constant));} 45 | HomogTransf & HomogTransf::operator/=(const double constant){return(*this=HomogTransf((Mat)*this/=constant));} 46 | HomogTransf HomogTransf::operator +(const HomogTransf &original)const {return (HomogTransf((Mat)*this+(Mat)original));} 47 | HomogTransf HomogTransf::operator -(const HomogTransf &original)const {return (HomogTransf((Mat)*this-(Mat)original));} 48 | HomogTransf HomogTransf::operator *(const HomogTransf &original)const {return (HomogTransf((Mat)*this*(Mat)original));} 49 | HomogTransf HomogTransf::operator *(const double constant)const {return (HomogTransf((Mat)*this*constant));} 50 | HomogTransf HomogTransf::operator /(const double constant)const {return (HomogTransf((Mat)*this/constant));} 51 | 52 | Vec HomogTransf::operator *(const Vec &original)const 53 | { 54 | if (original.nn==3) 55 | { 56 | Vec w(0.0,3); 57 | 58 | if(original.nn==3) 59 | { 60 | int i,j; 61 | for(i=0;i<3;i++) 62 | { 63 | for(j=0;j<3;j++) w[i]+=v[i][j]*original[j]; 64 | w[i]+=v[i][3]; 65 | } 66 | } 67 | return w; 68 | } 69 | else 70 | return (((Mat)(*this))*original); 71 | } 72 | 73 | void HomogTransf::setRotation(const RotMat &rot) 74 | { 75 | for(int i=0;i<3;i++) 76 | for(int j=0;j<3;j++) v[i][j]=rot[i][j]; 77 | } 78 | 79 | void HomogTransf::setTranslation(const Vec &trans) 80 | { 81 | v[0][3]=trans[0]; 82 | v[1][3]=trans[1]; 83 | v[2][3]=trans[2]; 84 | } 85 | 86 | RotMat HomogTransf::getRotation() const 87 | { 88 | RotMat r; 89 | for (int i=0;i<3;i++) 90 | for (int j=0;j<3;j++) 91 | r[i][j] = v[i][j]; 92 | return r; 93 | } 94 | 95 | Vec HomogTransf::getTranslation() const 96 | { 97 | Vec trans(3); 98 | trans[0] = v[0][3]; 99 | trans[1] = v[1][3]; 100 | trans[2] = v[2][3]; 101 | return trans; 102 | } 103 | 104 | 105 | void HomogTransf::setScrew(const Vec &point, const Vec &director_vector, const double displacement, const double angle) 106 | { 107 | double ca,sa,va; 108 | 109 | ca = cos(angle); sa = sin (angle);va = 1-cos(angle); 110 | 111 | v[0][0] = director_vector[0]*director_vector[0]*va+ca; v[0][1] = director_vector[0]*director_vector[1]*va-director_vector[2]*sa; v[0][2] = director_vector[0]*director_vector[2]*va+director_vector[1]*sa; 112 | v[1][0] = director_vector[0]*director_vector[1]*va+director_vector[2]*sa; v[1][1] = director_vector[1]*director_vector[1]*va+ca; v[1][2] = director_vector[1]*director_vector[2]*va-director_vector[0]*sa; 113 | v[2][0] = director_vector[0]*director_vector[2]*va-director_vector[1]*sa; v[2][1] = director_vector[1]*director_vector[2]*va+director_vector[0]*sa; v[2][2] = director_vector[2]*director_vector[2]*va+ca; 114 | v[3][0] = 0.0; v[3][1] = 0.0; v[3][2] = 0.0; 115 | 116 | v[0][3] = displacement*director_vector[0]-point[0]*(v[0][0]-1)-point[1]*v[0][1]-point[2]*v[0][2]; 117 | v[1][3] = displacement*director_vector[1]-point[0]*v[1][0]-point[1]*(v[1][1]-1)-point[2]*v[1][2]; 118 | v[2][3] = displacement*director_vector[2]-point[0]*v[2][0]-point[1]*v[2][1]-point[2]*(v[2][2]-1); 119 | v[3][3] = 1.0; 120 | } 121 | 122 | HomogTransf HomogTransf::inv() const 123 | { 124 | HomogTransf h; 125 | 126 | h[0][0] = v[0][0]; h[0][1] = v[1][0]; h[0][2] = v[2][0]; 127 | h[1][0] = v[0][1]; h[1][1] = v[1][1]; h[1][2] = v[2][1]; 128 | h[2][0] = v[0][2]; h[2][1] = v[1][2]; h[2][2] = v[2][2]; 129 | 130 | h[0][3] = - (v[0][3]*v[0][0]) - (v[1][3]*v[1][0]) - (v[2][3]*v[2][0]); 131 | h[1][3] = - (v[0][3]*v[0][1]) - (v[1][3]*v[1][1]) - (v[2][3]*v[2][1]); 132 | h[2][3] = - (v[0][3]*v[0][2]) - (v[1][3]*v[1][2]) - (v[2][3]*v[2][2]); 133 | 134 | h[3][0] = 0.0; h[3][1] = 0.0; h[3][2] = 0.0; h[3][3] = 1.0; 135 | 136 | return h; 137 | } 138 | -------------------------------------------------------------------------------- /abb_node/packages/matVec/HomogTransf.h: -------------------------------------------------------------------------------- 1 | #if !defined(HOMOGTRANSF_INCLUDED) 2 | #define HOMOGTRANSF_INCLUDED 3 | 4 | class HomogTransf : public Mat 5 | { 6 | public: 7 | //Constructors 8 | HomogTransf(); // Null translation and rotation. 9 | HomogTransf(double const *values); 10 | HomogTransf(char const *string); 11 | HomogTransf(const HomogTransf &original); // Copy constructor. 12 | HomogTransf(const RotMat &rot, const Vec &trans); 13 | HomogTransf(const Mat &original); // Conversion constructor. 14 | 15 | //Operators 16 | //(Explicitely Inherited for preserving the output class label) 17 | HomogTransf& operator=(const double constant); 18 | HomogTransf& operator+=(const HomogTransf &original); 19 | HomogTransf& operator-=(const HomogTransf &original); 20 | HomogTransf& operator*=(const double constant); 21 | HomogTransf& operator/=(const double constant); 22 | HomogTransf operator +(const HomogTransf &original) const; 23 | HomogTransf operator -(const HomogTransf &original) const; 24 | HomogTransf operator *(const HomogTransf &original) const; 25 | HomogTransf operator *(const double constant) const; 26 | HomogTransf operator /(const double constant) const; 27 | //New Operator 28 | Vec operator *(const Vec &original) const; //If original is a 4-vector: Standard product. 29 | //If original is a 3-vector: Transformation of a point in R3. 30 | //Interface 31 | void setTranslation(const Vec &trans); 32 | void setRotation(const RotMat &rot); 33 | RotMat getRotation() const; 34 | Vec getTranslation() const; 35 | 36 | //Transformations 37 | void setScrew(const Vec &point,const Vec &director_vector,const double displacement,const double angle); 38 | 39 | //Algebra 40 | HomogTransf inv() const; 41 | 42 | }; 43 | 44 | #endif // !defined(HOMOGTRANSF_INCLUDED) 45 | -------------------------------------------------------------------------------- /abb_node/packages/matVec/Mat.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "Vec.h" 6 | #include "Mat.h" 7 | 8 | 9 | /** \brief Default constructor. 10 | * Zero-size matrix. 11 | * 12 | * This constructor creates an empty matrix. No memory space is allocated, so 13 | * accessing any element will cause an error. 14 | */ 15 | Mat::Mat() 16 | { 17 | nn=0; 18 | mm=0; 19 | v=0; 20 | } 21 | 22 | /** \brief Constructs an NxM matrix. 23 | * \param n number of rows. 24 | * \param m number of columns. 25 | * 26 | * This constructor creates a matrix of n rows by m columns. The matrix's elements 27 | * are not initialized. 28 | */ 29 | Mat::Mat(const int n, const int m) 30 | { 31 | if ((n>0) && (m>0)) 32 | { 33 | nn = n; 34 | mm = m; 35 | v = new double*[n]; 36 | v[0] = new double[m*n]; 37 | for (int i=1; i0) && (m>0)) 55 | { 56 | int i,j; 57 | nn = n; 58 | mm = m; 59 | v = new double*[n]; 60 | v[0] = new double[m*n]; 61 | for (i=1; i0) && (m>0)) 82 | { 83 | int i,j; 84 | nn = n; 85 | mm = m; 86 | v = new double*[n]; 87 | v[0] = new double[m*n]; 88 | for (i=1; i0) && (m>0)) 113 | { 114 | int i,j,k,l,error; 115 | char aux[30]; // Stores one of the floats in string. 116 | 117 | nn = n; 118 | mm = m; 119 | v = new double*[n]; 120 | v[0] = new double[m*n]; 121 | for (i=1;i 0) 143 | { 144 | // Conversion ok. 145 | if ( (k==nn-1) && (l==mm-1) ) 146 | { 147 | // More reals in string than element in matrix. Matrix filled. 148 | return; 149 | } 150 | // Go to next element of the matrix. 151 | l++; 152 | if (l==m) 153 | { 154 | l=0; 155 | k++; 156 | } 157 | } 158 | // Reset aux. 159 | j=0; 160 | while ( (aux[j] != 0) && (j<30) ) 161 | { 162 | aux[j] = 0; 163 | j++; 164 | } 165 | j=0; 166 | } 167 | i++; // Next element of string. 168 | } 169 | if (string[i-1]!=0) 170 | { 171 | // Do the last conversion if necessary. 172 | sscanf(aux,"%lf",&v[k][l]); 173 | } 174 | } 175 | else Mat(); 176 | } 177 | 178 | /** \brief Copy constructor. 179 | * \param rhs matrix to be copied to the new one. 180 | * 181 | * This constructor creates a copy of rhs. 182 | */ 183 | Mat::Mat(const Mat &origin_matrix) 184 | { 185 | nn=origin_matrix.nn; 186 | mm=origin_matrix.mm; 187 | v=new double*[nn]; 188 | v[0] = new double[mm*nn]; 189 | int i,j; 190 | for (i=1; i=number of rows). 213 | * 214 | * Method that gives access to any induvidual element of the matrix. The following 215 | * example shows how to change tha value of an individual element of a matrix: 216 | * \code 217 | * Mat_DP m("1 2 3 4", 2, 2); 218 | * m[0][1] = 25; \endcode 219 | */ 220 | double* Mat::operator [](const int i) const 221 | { 222 | if ((i>=0) && (imax_pivot) 563 | { 564 | max_pivot=fabs(w[k][i]); 565 | pivot=k; 566 | } 567 | } 568 | 569 | //If we have found a greater pivote that the original we make the permutation 570 | if(pivot!=i) 571 | { 572 | sign=sign*(-1); 573 | //We have to interchange rows i and pivot of matrix A 574 | for(j=0; j= 0 ; i--) 664 | { 665 | x[i] = y[i]; 666 | for(j = i + 1 ; j < nn ; j++) 667 | x[i] = x[i] - U[i][j]*x[j]; 668 | x[i] = x[i]/U[i][i]; 669 | } 670 | } 671 | return x; 672 | } 673 | 674 | Vec Mat::LDUsolve(const Mat &L, const Mat &D, const Mat &U, const Mat &P, const Vec &b) const 675 | { 676 | Vec x; 677 | if((nn==mm)&&(nn==b.nn)) 678 | { 679 | int i,j; 680 | Vec b2=P*b; 681 | 682 | x=Vec(0.0,mm); 683 | Vec y(0.0,nn); 684 | 685 | y[0] = b2[0]; 686 | for (i = 1 ; i < nn ; i++) 687 | { 688 | y[i] = b2[i]; 689 | for(j = 0 ; j < i ; j++) 690 | y[i] = y[i] - L[i][j]*y[j]; 691 | } 692 | 693 | for(i=0;i= 0 ; i--) 698 | { 699 | x[i] = y[i]; 700 | for(j = i + 1 ; j < nn ; j++) 701 | x[i] = x[i] - U[i][j]*x[j]; 702 | x[i] = x[i]/U[i][i]; 703 | } 704 | } 705 | return x; 706 | } 707 | 708 | Mat Mat::LDUinverse() const 709 | { 710 | Mat w(nn,mm); 711 | if(nn==mm) 712 | { 713 | Mat L, D, U, P; 714 | LDU(L,D,U,P); 715 | 716 | int i,j; 717 | Vec aux; 718 | Vec b(0.0,mm); 719 | for(j=0;j=0)&&(n=0)&&(m=0)&&(n=0)&&(m bt) { ct = bt / at; result = at * sqrt(1.0 + ct * ct); } 895 | else if (bt > 0.0) { ct = at / bt; result = bt * sqrt(1.0 + ct * ct); } 896 | else result = 0.0; 897 | return(result); 898 | } 899 | 900 | int Mat::SVD(Mat &U, Vec &sigma, Mat &V) const 901 | { 902 | int flag, i, its, j, jj, k, l, nm; 903 | double c, f, h, s, x, y, z; 904 | double anorm = 0.0, g = 0.0, scale = 0.0; 905 | double *rv1; 906 | if (nn < mm) 907 | { 908 | fprintf(stderr, "#rows must be >= #cols \n"); 909 | return(0); 910 | } 911 | 912 | U = Mat(*this); 913 | sigma = Vec(0.0,mm); 914 | V = Mat(0.0,mm,mm); 915 | 916 | rv1 = (double *)malloc((unsigned int) mm*sizeof(double)); 917 | l=0; 918 | /* Householder reduction to bidiagonal form */ 919 | for (i = 0; i < mm; i++) 920 | { 921 | /* left-hand reduction */ 922 | l = i + 1; 923 | rv1[i] = scale * g; 924 | g = s = scale = 0.0; 925 | if (i < nn) 926 | { 927 | for (k = i; k < nn; k++) 928 | scale += fabs((double)U[k][i]); 929 | if (scale) 930 | { 931 | for (k = i; k < nn; k++) 932 | { 933 | U[k][i] = (float)((double)U[k][i]/scale); 934 | s += ((double)U[k][i] * (double)U[k][i]); 935 | } 936 | f = (double)U[i][i]; 937 | g = -SIGN(f)*sqrt(s); 938 | h = f * g - s; 939 | U[i][i] = (float)(f - g); 940 | if (i != mm - 1) 941 | { 942 | for (j = l; j < mm; j++) 943 | { 944 | for (s = 0.0, k = i; k < nn; k++) 945 | s += ((double)U[k][i] * (double)U[k][j]); 946 | f = s / h; 947 | for (k = i; k < nn; k++) 948 | U[k][j] += (float)(f * (double)U[k][i]); 949 | } 950 | } 951 | for (k = i; k < nn; k++) 952 | U[k][i] = (float)((double)U[k][i]*scale); 953 | } 954 | } 955 | sigma[i] = (float)(scale * g); 956 | 957 | /* right-hand reduction */ 958 | g = s = scale = 0.0; 959 | if (i < nn && i != mm - 1) 960 | { 961 | for (k = l; k < mm; k++) 962 | scale += fabs((double)U[i][k]); 963 | if (scale) 964 | { 965 | for (k = l; k < mm; k++) 966 | { 967 | U[i][k] = (float)((double)U[i][k]/scale); 968 | s += ((double)U[i][k] * (double)U[i][k]); 969 | } 970 | f = (double)U[i][l]; 971 | g = -SIGN(f)*sqrt(s); 972 | h = f * g - s; 973 | U[i][l] = (float)(f - g); 974 | for (k = l; k < mm; k++) 975 | rv1[k] = (double)U[i][k] / h; 976 | if (i != nn - 1) 977 | { 978 | for (j = l; j < nn; j++) 979 | { 980 | for (s = 0.0, k = l; k < mm; k++) 981 | s += ((double)U[j][k] * (double)U[i][k]); 982 | for (k = l; k < mm; k++) 983 | U[j][k] += (float)(s * rv1[k]); 984 | } 985 | } 986 | for (k = l; k < mm; k++) 987 | U[i][k] = (float)((double)U[i][k]*scale); 988 | } 989 | } 990 | anorm = MAX(anorm, (fabs((double)sigma[i]) + fabs(rv1[i]))); 991 | } 992 | 993 | /* accumulate the right-hand transformation */ 994 | for (i = mm - 1; i >= 0; i--) 995 | { 996 | if (i < mm - 1) 997 | { 998 | if (g) 999 | { 1000 | for (j = l; j < mm; j++) 1001 | V[j][i] = (float)(((double)U[i][j] / (double)U[i][l]) / g); 1002 | /* double division to avoid underflow */ 1003 | for (j = l; j < mm; j++) 1004 | { 1005 | for (s = 0.0, k = l; k < mm; k++) 1006 | s += ((double)U[i][k] * (double)V[k][j]); 1007 | for (k = l; k < mm; k++) 1008 | V[k][j] += (float)(s * (double)V[k][i]); 1009 | } 1010 | } 1011 | for (j = l; j < mm; j++) 1012 | V[i][j] = V[j][i] = 0.0; 1013 | } 1014 | V[i][i] = 1.0; 1015 | g = rv1[i]; 1016 | l = i; 1017 | } 1018 | 1019 | /* accumulate the left-hand transformation */ 1020 | for (i = mm - 1; i >= 0; i--) 1021 | { 1022 | l = i + 1; 1023 | g = (double)sigma[i]; 1024 | if (i < mm - 1) 1025 | for (j = l; j < mm; j++) 1026 | U[i][j] = 0.0; 1027 | if (g) 1028 | { 1029 | g = 1.0 / g; 1030 | if (i != mm - 1) 1031 | { 1032 | for (j = l; j < mm; j++) 1033 | { 1034 | for (s = 0.0, k = l; k < nn; k++) 1035 | s += ((double)U[k][i] * (double)U[k][j]); 1036 | f = (s / (double)U[i][i]) * g; 1037 | for (k = i; k < nn; k++) 1038 | U[k][j] += (float)(f * (double)U[k][i]); 1039 | } 1040 | } 1041 | for (j = i; j < nn; j++) 1042 | U[j][i] = (float)((double)U[j][i]*g); 1043 | } 1044 | else 1045 | { 1046 | for (j = i; j < nn; j++) 1047 | U[j][i] = 0.0; 1048 | } 1049 | ++U[i][i]; 1050 | } 1051 | 1052 | /* diagonalize the bidiagonal form */ 1053 | for (k = mm - 1; k >= 0; k--) 1054 | { /* loop over singular values */ 1055 | for (its = 0; its < 30; its++) 1056 | { /* loop over allowed iterations */ 1057 | flag = 1; 1058 | for (l = k; l >= 0; l--) 1059 | { /* test for splitting */ 1060 | nm = l - 1; 1061 | if (fabs(rv1[l]) + anorm == anorm) 1062 | { 1063 | flag = 0; 1064 | break; 1065 | } 1066 | if (fabs((double)sigma[nm]) + anorm == anorm) 1067 | break; 1068 | } 1069 | if (flag) 1070 | { 1071 | c = 0.0; 1072 | s = 1.0; 1073 | for (i = l; i <= k; i++) 1074 | { 1075 | f = s * rv1[i]; 1076 | if (fabs(f) + anorm != anorm) 1077 | { 1078 | g = (double)sigma[i]; 1079 | h = PYTHAG(f, g); 1080 | sigma[i] = (float)h; 1081 | h = 1.0 / h; 1082 | c = g * h; 1083 | s = (- f * h); 1084 | for (j = 0; j < nn; j++) 1085 | { 1086 | y = (double)U[j][nm]; 1087 | z = (double)U[j][i]; 1088 | U[j][nm] = (float)(y * c + z * s); 1089 | U[j][i] = (float)(z * c - y * s); 1090 | } 1091 | } 1092 | } 1093 | } 1094 | z = (double)sigma[k]; 1095 | if (l == k) 1096 | { /* convergence */ 1097 | if (z < 0.0) 1098 | { /* make singular value nommegative */ 1099 | sigma[k] = (float)(-z); 1100 | for (j = 0; j < mm; j++) 1101 | V[j][k] = (-V[j][k]); 1102 | } 1103 | break; 1104 | } 1105 | if (its >= 30) { 1106 | free((void*) rv1); 1107 | fprintf(stderr, "No convergence after 30,000! iterations \n"); 1108 | return(0); 1109 | } 1110 | 1111 | /* shift from bottom 2 x 2 minor */ 1112 | x = (double)sigma[l]; 1113 | nm = k - 1; 1114 | y = (double)sigma[nm]; 1115 | g = rv1[nm]; 1116 | h = rv1[k]; 1117 | f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y); 1118 | g = PYTHAG(f, 1.0); 1119 | f = ((x - z) * (x + z) + h * ((y / (f + SIGN(f)*fabs(g))) - h)) / x; 1120 | 1121 | /* next QR transformation */ 1122 | c = s = 1.0; 1123 | for (j = l; j <= nm; j++) 1124 | { 1125 | i = j + 1; 1126 | g = rv1[i]; 1127 | y = (double)sigma[i]; 1128 | h = s * g; 1129 | g = c * g; 1130 | z = PYTHAG(f, h); 1131 | rv1[j] = z; 1132 | c = f / z; 1133 | s = h / z; 1134 | f = x * c + g * s; 1135 | g = g * c - x * s; 1136 | h = y * s; 1137 | y = y * c; 1138 | for (jj = 0; jj < mm; jj++) 1139 | { 1140 | x = (double)V[jj][j]; 1141 | z = (double)V[jj][i]; 1142 | V[jj][j] = (float)(x * c + z * s); 1143 | V[jj][i] = (float)(z * c - x * s); 1144 | } 1145 | z = PYTHAG(f, h); 1146 | sigma[j] = (float)z; 1147 | if (z) 1148 | { 1149 | z = 1.0 / z; 1150 | c = f * z; 1151 | s = h * z; 1152 | } 1153 | f = (c * g) + (s * y); 1154 | x = (c * y) - (s * g); 1155 | for (jj = 0; jj < nn; jj++) 1156 | { 1157 | y = (double)U[jj][j]; 1158 | z = (double)U[jj][i]; 1159 | U[jj][j] = (float)(y * c + z * s); 1160 | U[jj][i] = (float)(z * c - y * s); 1161 | } 1162 | } 1163 | rv1[l] = 0.0; 1164 | rv1[k] = f; 1165 | sigma[k] = (float)x; 1166 | } 1167 | } 1168 | free((void*) rv1); 1169 | return(1); 1170 | } 1171 | -------------------------------------------------------------------------------- /abb_node/packages/matVec/Mat.h: -------------------------------------------------------------------------------- 1 | #if !defined(MAT_INCLUDED) 2 | #define MAT_INCLUDED 3 | 4 | #include 5 | 6 | class Mat 7 | { 8 | public: 9 | int nn; ///< Number of rows. Index range is 0..nn-1. 10 | int mm; ///< Number of columns. Index range is 0..mm-1. 11 | double **v; ///< Storage of data. 12 | int error; //Error type. 13 | 14 | // Constructors. 15 | Mat(); 16 | Mat(const int n, const int m); 17 | Mat(const double constant, const int n, const int m); 18 | Mat(double const * values, const int n, const int m); 19 | Mat(char const * string, const int n, const int m); 20 | Mat(const Mat &origin_matrix); 21 | 22 | //Destructor 23 | ~Mat(); 24 | 25 | //Operators 26 | double* operator[](const int i) const; 27 | Mat& operator=(const Mat &original); 28 | Mat& operator=(const double constant); 29 | Mat& operator+=(const Mat &original); 30 | Mat& operator-=(const Mat &original); 31 | Mat operator +(const Mat &original) const; 32 | Mat operator -(const Mat &original) const; 33 | Mat operator *(const Mat &original) const; 34 | Vec operator *(const Vec &original) const; 35 | 36 | Mat operator -() const; 37 | 38 | Mat& operator+=(const double constant); 39 | Mat& operator-=(const double constant); 40 | Mat& operator*=(const double constant); 41 | Mat& operator/=(const double constant); 42 | Mat operator +(const double constant) const; 43 | Mat operator -(const double constant) const; 44 | Mat operator *(const double constant) const; 45 | Mat operator /(const double constant) const; 46 | 47 | friend std::ostream& operator<<(std::ostream& os, const Mat &orig); 48 | 49 | // LDU decomposition. 50 | Mat LDU(Vec &permutations, int &sign) const; 51 | void LDU(Mat &L, Mat &D, Mat &U, Mat &P) const; 52 | Vec LDUsolve(const Vec &b) const; 53 | Vec LDUsolve(const Mat &L, const Mat &D, const Mat &U, const Mat &P,const Vec &b) const; 54 | Mat LDUinverse() const; 55 | double LDUdet() const; 56 | Vec LSsolve(const Vec &b) const; 57 | 58 | //SVD decomposition. 59 | int SVD(Mat &U, Vec &sigma, Mat &V) const; 60 | 61 | //Linear Algebra 62 | Mat transp() const; 63 | Mat inv() const; 64 | double det() const; 65 | Vec getRow(const int n) const; 66 | Vec getCol(const int m) const; 67 | void setRow(const int n, const Vec &row); 68 | void setCol(const int m, const Vec &col); 69 | 70 | //Statistics 71 | double mean() const; 72 | double variance() const; 73 | double stdev() const; 74 | 75 | private: 76 | double PYTHAG(double a, double b) const; //For SVD decomposition 77 | 78 | }; 79 | 80 | #endif // !defined(MAT_INCLUDED) 81 | -------------------------------------------------------------------------------- /abb_node/packages/matVec/Polynom.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "Vec.h" 4 | #include "Mat.h" 5 | #include "Polynom.h" 6 | 7 | Polynom::Polynom() : Vec() {} 8 | 9 | Polynom::Polynom(const int n) : Vec(n+1) {} 10 | 11 | Polynom::Polynom(const double constant, const int n) : Vec(constant,n+1) {} 12 | 13 | Polynom::Polynom(double const *values, const int n) : Vec(values,n+1) {} 14 | 15 | Polynom::Polynom(char const *string, const int n) : Vec(string,n+1) {} 16 | 17 | Polynom::Polynom(const Vec &origin_vector) : Vec(origin_vector) {} 18 | 19 | Polynom::Polynom(const Polynom &origin_polynom) : Vec((Vec)origin_polynom) {} 20 | 21 | int Polynom::degree() const 22 | { 23 | return(nn-1); 24 | } 25 | 26 | Polynom & Polynom::operator =(const double constant){return(*this=Polynom((Vec)*this=constant));} 27 | Polynom Polynom::operator -() const {return(Polynom(-(Vec)*this));} 28 | Polynom Polynom::operator *(const double constant){return (Polynom((Vec)*this*constant));} 29 | Polynom Polynom::operator /(const double constant){return (Polynom((Vec)*this/constant));} 30 | 31 | Polynom Polynom::operator +(const Polynom &original) 32 | { 33 | if(nn>original.nn) 34 | { 35 | Polynom w(*this); 36 | int i; 37 | for(i=0;ioriginal.nn) 59 | { 60 | Polynom w(*this); 61 | int i; 62 | for(i=0;i 2 | 3 | #include "Vec.h" 4 | #include "Mat.h" 5 | #include "RotMat.h" 6 | #include "Quaternion.h" 7 | 8 | Quaternion::Quaternion() : Vec(4) {} 9 | 10 | Quaternion::Quaternion(const double constant) : Vec(constant,4) {} 11 | 12 | Quaternion::Quaternion(double const *values) : Vec(values,4) {} 13 | 14 | Quaternion::Quaternion(char const *string) : Vec(string,4) {} 15 | 16 | Quaternion::Quaternion(double const q0, Vec const qv) : Vec(4) 17 | { 18 | v[0] = q0; 19 | v[1] = qv[0]; 20 | v[2] = qv[1]; 21 | v[3] = qv[2]; 22 | } 23 | 24 | Quaternion::Quaternion(const Vec &origin_vector) : Vec(4) 25 | { 26 | if (origin_vector.nn==4) 27 | { 28 | for(int i=0;i<4;i++) 29 | v[i]=origin_vector[i]; 30 | } 31 | 32 | } 33 | 34 | Quaternion::Quaternion(const Quaternion &origin_quaternion) : Vec((Vec)origin_quaternion) {} 35 | 36 | Quaternion & Quaternion::operator =(const double constant){return(*this=Quaternion((Vec)*this=constant));} 37 | Quaternion & Quaternion::operator +=(const Quaternion &original){return(*this=Quaternion((Vec)*this+=(Vec)original));} 38 | Quaternion & Quaternion::operator -=(const Quaternion &original){return(*this=Quaternion((Vec)*this-=(Vec)original));} 39 | Quaternion Quaternion::operator +(const Quaternion &original)const {return (Quaternion((Vec)*this+(Vec)original));} 40 | Quaternion Quaternion::operator -(const Quaternion &original)const {return (Quaternion((Vec)*this-(Vec)original));} 41 | Quaternion Quaternion::operator -()const {return(Quaternion(-(Vec)*this));} 42 | Quaternion Quaternion::operator *(const double constant)const {return (Quaternion((Vec)*this*constant));} 43 | Quaternion Quaternion::operator /(const double constant)const {return (Quaternion((Vec)*this/constant));} 44 | Quaternion Quaternion::operator ^(const Quaternion &original)const 45 | { 46 | Quaternion w; 47 | w[0]=v[0]*original[0] - v[1]*original[1] - v[2]*original[2] - v[3]*original[3]; 48 | w[1]=v[0]*original[1] + v[1]*original[0] + v[2]*original[3] - v[3]*original[2]; 49 | w[2]=v[0]*original[2] - v[1]*original[3] + v[2]*original[0] + v[3]*original[1]; 50 | w[3]=v[0]*original[3] + v[1]*original[2] - v[2]*original[1] + v[3]*original[0]; 51 | 52 | return w; 53 | } 54 | double Quaternion::operator *(const Quaternion &original)const 55 | { 56 | double e; 57 | e=v[0]*original[0] + v[1]*original[1] + v[2]*original[2] + v[3]*original[3]; 58 | 59 | return e; 60 | } 61 | 62 | Quaternion Quaternion::conjugate()const 63 | { 64 | Quaternion w; 65 | w[0]=v[0]; 66 | w[1]=-v[1]; 67 | w[2]=-v[2]; 68 | w[3]=-v[3]; 69 | 70 | return w; 71 | } 72 | 73 | double Quaternion::getScalar()const 74 | { 75 | return v[0]; 76 | } 77 | 78 | Vec Quaternion::getVector()const 79 | { 80 | Vec w(3); 81 | w[0]=v[1]; 82 | w[1]=v[2]; 83 | w[2]=v[3]; 84 | return w; 85 | } 86 | 87 | void Quaternion::setScalar(const double s) 88 | { 89 | v[0]=s; 90 | } 91 | 92 | void Quaternion::setVector(const Vec &vector) 93 | { 94 | if(vector.nn==3) 95 | { 96 | v[1]=vector[0]; 97 | v[2]=vector[1]; 98 | v[3]=vector[2]; 99 | } 100 | } 101 | 102 | Quaternion Quaternion::inverse()const 103 | { 104 | double n=norm(); 105 | if (n!=0) 106 | return ((this->conjugate())/(n*n)); 107 | else 108 | return (Quaternion(0.0)); 109 | } 110 | 111 | Mat Quaternion::leftMat()const 112 | { 113 | Mat w(0.0,4,4); 114 | w[0][0]=v[0] ; w[0][1]=-v[1] ; w[0][2]=-v[2] ; w[0][3]=-v[3] ; 115 | w[1][0]=v[1] ; w[1][1]=v[0] ; w[1][2]=-v[3] ; w[1][3]=v[2] ; 116 | w[2][0]=v[2] ; w[2][1]=v[3] ; w[2][2]=v[0] ; w[2][3]=-v[1] ; 117 | w[3][0]=v[3] ; w[3][1]=-v[2] ; w[3][2]=v[1] ; w[3][3]=v[0] ; 118 | 119 | return w; 120 | } 121 | 122 | Mat Quaternion::rightMat()const 123 | { 124 | Mat w(0.0,4,4); 125 | w[0][0]=v[0] ; w[0][1]=-v[1] ; w[0][2]=-v[2] ; w[0][3]=-v[3] ; 126 | w[1][0]=v[1] ; w[1][1]=v[0] ; w[1][2]=v[3] ; w[1][3]=-v[2] ; 127 | w[2][0]=v[2] ; w[2][1]=-v[3] ; w[2][2]=v[0] ; w[2][3]=v[1] ; 128 | w[3][0]=v[3] ; w[3][1]=v[2] ; w[3][2]=-v[1] ; w[3][3]=v[0] ; 129 | 130 | return w; 131 | } 132 | 133 | double Quaternion::getAngle()const 134 | { 135 | Vec vector=getVector(); 136 | double s=getScalar(); 137 | double a=2*atan2(vector.norm(),s); 138 | if (a>PI) 139 | a=2*PI-a; 140 | return a; 141 | } 142 | 143 | Vec Quaternion::getAxis()const 144 | { 145 | double n=getVector().norm(); 146 | if (n!=0) 147 | return(getVector()/n); 148 | else 149 | return(Vec("0.0 0.0 0.0",3)); 150 | } 151 | 152 | RotMat Quaternion::getRotMat()const 153 | { 154 | RotMat w; 155 | w[0][0]=v[0]*v[0]+v[1]*v[1]-v[2]*v[2]-v[3]*v[3]; 156 | w[0][1]=2*(v[1]*v[2]-v[0]*v[3]); 157 | w[0][2]=2*(v[1]*v[3]+v[0]*v[2]); 158 | w[1][0]=2*(v[1]*v[2]+v[0]*v[3]); 159 | w[1][1]=v[0]*v[0]-v[1]*v[1]+v[2]*v[2]-v[3]*v[3]; 160 | w[1][2]=2*(v[2]*v[3]-v[0]*v[1]); 161 | w[2][0]=2*(v[1]*v[3]-v[0]*v[2]); 162 | w[2][1]=2*(v[2]*v[3]+v[0]*v[1]); 163 | w[2][2]=v[0]*v[0]-v[1]*v[1]-v[2]*v[2]+v[3]*v[3]; 164 | return w; 165 | } 166 | -------------------------------------------------------------------------------- /abb_node/packages/matVec/Quaternion.h: -------------------------------------------------------------------------------- 1 | #if !defined(QUATERNION_INCLUDED) 2 | #define QUATERNION_INCLUDED 3 | 4 | 5 | //class Vec; 6 | class RotMat; 7 | 8 | class Quaternion: public Vec 9 | { 10 | public: 11 | //Constructors 12 | Quaternion(); 13 | Quaternion(const double constant); //Initialize to constant value 14 | Quaternion(double const *values);// Initialize to values in C-style array a 15 | Quaternion(char const *string); //Initialize to values in string 16 | Quaternion(double const q0, Vec const v); //Initialize with scalar and vector 17 | Quaternion(const Vec &origin_vector); // Conversion constructor 18 | Quaternion(const Quaternion &origin_quaternion); //Copy constructor 19 | 20 | //Operators 21 | //(Explicitely Inherited for preserving the output class label) 22 | Quaternion & operator =(const double constant); //assign a to every element 23 | Quaternion & operator +=(const Quaternion &original); 24 | Quaternion & operator -=(const Quaternion &original); 25 | Quaternion operator +(const Quaternion &original) const; 26 | Quaternion operator -(const Quaternion &original) const; 27 | Quaternion operator -() const; 28 | Quaternion operator *(const double constant) const; 29 | Quaternion operator /(const double constant) const; 30 | //(New) 31 | Quaternion operator ^(const Quaternion &original) const; // Quaternion product. 32 | double operator *(const Quaternion &original) const; 33 | 34 | //Quaternion Algebra 35 | Quaternion conjugate() const; 36 | Quaternion inverse() const; 37 | double getScalar() const; 38 | Vec getVector() const; 39 | void setScalar(const double s); 40 | void setVector(const Vec &vector); 41 | Mat leftMat() const; 42 | Mat rightMat() const; 43 | 44 | //Transformation 45 | double getAngle() const; 46 | Vec getAxis() const; 47 | RotMat getRotMat() const; 48 | }; 49 | 50 | #endif // !defined(QUATERNION_INCLUDED) 51 | -------------------------------------------------------------------------------- /abb_node/packages/matVec/RotMat.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "Vec.h" 4 | #include "Mat.h" 5 | #include "Quaternion.h" 6 | #include "RotMat.h" 7 | 8 | RotMat::RotMat() : Mat(0.0,3,3) 9 | { 10 | for(int i=0;i<3;i++) v[i][i]=1.0; 11 | } 12 | RotMat::RotMat(const RotMat &original) : Mat(original){} 13 | 14 | RotMat::RotMat(double const *values) : Mat(values,3,3){} 15 | 16 | RotMat::RotMat(char const *string) : Mat(string,3,3){} 17 | 18 | RotMat::RotMat(const Vec X, const Vec Y, const Vec Z) : Mat(0.0,3,3) 19 | { 20 | setRefFrame(X,Y,Z); 21 | } 22 | 23 | RotMat::RotMat(const Mat &original) : Mat(0.0,3,3) 24 | { 25 | if ( (original.nn==3) && (original.mm==3) ) 26 | { 27 | for(int i=0;i<3;i++) 28 | { 29 | for(int j=0;j<3;j++) 30 | { 31 | v[i][j]=original[i][j]; 32 | } 33 | } 34 | } 35 | } 36 | 37 | 38 | //(Explicitely Inherited for preserving the output class label) 39 | RotMat & RotMat::operator=(const double constant){return(*this=RotMat((Mat)*this=constant));} 40 | RotMat & RotMat::operator+=(const RotMat &original){return(*this=RotMat((Mat)*this+=(Mat)original));} 41 | RotMat & RotMat::operator-=(const RotMat &original){return(*this=RotMat((Mat)*this-=(Mat)original));} 42 | RotMat & RotMat::operator*=(const double constant){return(*this=RotMat((Mat)*this*=constant));} 43 | RotMat & RotMat::operator/=(const double constant){return(*this=RotMat((Mat)*this/=constant));} 44 | RotMat RotMat::operator +(const RotMat &original)const {return (RotMat((Mat)*this+(Mat)original));} 45 | RotMat RotMat::operator -(const RotMat &original)const {return (RotMat((Mat)*this-(Mat)original));} 46 | RotMat RotMat::operator *(const RotMat &original)const {return (RotMat((Mat)*this*(Mat)original));} 47 | Vec RotMat::operator *(const Vec &original)const {return ((Mat)*this*original);} 48 | RotMat RotMat::operator *(const double constant)const {return (RotMat((Mat)*this*constant));} 49 | RotMat RotMat::operator /(const double constant)const {return (RotMat((Mat)*this/constant));} 50 | 51 | 52 | void RotMat::setRefFrame(const Vec &X, const Vec &Y, const Vec &Z) 53 | { 54 | setCol(0,X); 55 | setCol(1,Y); 56 | setCol(2,Z); 57 | } 58 | 59 | void RotMat::rotX(const double alfa) 60 | { 61 | v[0][0]=1; 62 | v[1][0]=0; 63 | v[2][0]=0; 64 | v[0][1]=0; 65 | v[1][1]=cos(alfa); 66 | v[2][1]=sin(alfa); 67 | v[0][2]=0; 68 | v[1][2]=-sin(alfa); 69 | v[2][2]=cos(alfa); 70 | } 71 | 72 | void RotMat::rotY(const double alfa) 73 | { 74 | v[0][0]=cos(alfa); 75 | v[1][0]=0; 76 | v[2][0]=-sin(alfa); 77 | v[0][1]=0; 78 | v[1][1]=1; 79 | v[2][1]=0; 80 | v[0][2]=sin(alfa); 81 | v[1][2]=0; 82 | v[2][2]=cos(alfa); 83 | } 84 | 85 | void RotMat::rotZ(const double alfa) 86 | { 87 | v[0][0]=cos(alfa); 88 | v[1][0]=sin(alfa); 89 | v[2][0]=0; 90 | v[0][1]=-sin(alfa); 91 | v[1][1]=cos(alfa); 92 | v[2][1]=0; 93 | v[0][2]=0; 94 | v[1][2]=0; 95 | v[2][2]=1; 96 | } 97 | 98 | void RotMat::setAxisAngle(const Vec &vector,const double alfa) 99 | { 100 | Vec auxvec=vector; 101 | auxvec.normalize(); 102 | 103 | double c,s,t1,t2; 104 | c = cos(alfa); 105 | s = sin(alfa); 106 | t1 = 1-c; 107 | t2 = auxvec[0]*t1; 108 | 109 | v[0][0]=t2*auxvec[0] + c; 110 | v[1][0]=t2*auxvec[1] + auxvec[2]*s; 111 | v[2][0]=t2*auxvec[2] - auxvec[1]*s; 112 | v[0][1]=t2*auxvec[1] - auxvec[2]*s; 113 | v[1][1]=auxvec[1]*auxvec[1]*t1 + c; 114 | v[2][1]=auxvec[1]*auxvec[2]*t1 + auxvec[0]*s; 115 | v[0][2]=t2*auxvec[2] + auxvec[1]*s; 116 | v[1][2]=auxvec[1]*auxvec[2]*t1 - auxvec[0]*s; 117 | v[2][2]=auxvec[2]*auxvec[2]*t1 + c; 118 | } 119 | 120 | double RotMat::getAngle() const 121 | { 122 | return(acos((v[0][0]+v[1][1]+v[2][2]-1)/2)); 123 | } 124 | 125 | Vec RotMat::getAxis() const 126 | { 127 | Vec w=Vec(3); 128 | 129 | double alpha,s,t1; 130 | alpha=getAngle(); 131 | s = sin(alpha); 132 | if (s != 0) { 133 | t1 = 1/(2*s); 134 | w[0] = (v[2][1]-v[1][2])*t1; 135 | w[1] = (v[0][2]-v[2][0])*t1; 136 | w[2] = (v[1][0]-v[0][1])*t1; 137 | } 138 | else { 139 | if (alpha == 0) { 140 | w[0] = 0; 141 | w[1] = 0; 142 | w[2] = 0; 143 | } 144 | else { 145 | w[0] = sqrt((v[0][0]+1)/2); 146 | w[1] = v[1][2]/v[0][2]*w[0]; 147 | w[2] = v[2][1]/v[0][1]*w[0]; 148 | } 149 | } 150 | 151 | return w; 152 | } 153 | 154 | Quaternion RotMat::getQuaternion() const 155 | { 156 | Quaternion q; 157 | double t0 = 1.0 + v[0][0] + v[1][1] + v[2][2]; 158 | double t1 = 1.0 + v[0][0] - v[1][1] - v[2][2]; 159 | double t2 = 1.0 - v[0][0] + v[1][1] - v[2][2]; 160 | double t3 = 1.0 - v[0][0] - v[1][1] + v[2][2]; 161 | 162 | if (t0 >= t1 && t0 >= t2 && t0 >= t3) 163 | { 164 | double r = sqrt(t0); 165 | double s = 0.5 / r; 166 | 167 | q[0] = 0.5 * r; 168 | q[1] = ( v[2][1] - v[1][2] ) * s; 169 | q[2] = ( v[0][2] - v[2][0] ) * s; 170 | q[3] = ( v[1][0] - v[0][1] ) * s; 171 | } 172 | else if (t1 >= t2 && t1 >= t3) 173 | { 174 | double r = sqrt(t1); 175 | double s = 0.5 / r; 176 | 177 | q[0] = (v[2][1] - v[1][2] ) * s; 178 | q[1] = 0.5 * r; 179 | q[2] = (v[0][1] + v[1][0] ) * s; 180 | q[3] = (v[0][2] + v[2][0] ) * s; 181 | } 182 | else if (t2 >= t3) 183 | { 184 | double r = sqrt(t2); 185 | double s = 0.5 / r; 186 | 187 | q[0] = (v[0][2] - v[2][0] ) * s; 188 | q[1] = (v[0][1] + v[1][0] ) * s; 189 | q[2] = 0.5 * r; 190 | q[3] = (v[1][2] + v[2][1] ) * s; 191 | } 192 | else 193 | { 194 | double r = sqrt(t3); 195 | double s = 0.5 / r; 196 | 197 | q[0] = (v[1][0] - v[0][1]) * s; 198 | q[1] = (v[0][2] + v[2][0] ) * s; 199 | q[2] = (v[1][2] + v[2][1] ) * s; 200 | q[3] = 0.5 * r; 201 | } 202 | 203 | return q; 204 | 205 | /* 206 | 207 | Quaternion q; 208 | double trace = v[0][0] + v[1][1] + v[2][2] + 1.0; 209 | if( (trace - 1.0) > TOLERANCE ) 210 | { 211 | double s = 0.5 / sqrt(trace); 212 | q[0] = 0.25 / s; 213 | q[1] = ( v[2][1] - v[1][2] ) * s; 214 | q[2] = ( v[0][2] - v[2][0] ) * s; 215 | q[3] = ( v[1][0] - v[0][1] ) * s; 216 | } 217 | else 218 | { 219 | if ( v[0][0] > v[1][1] && v[0][0] > v[2][2] ) 220 | { 221 | double s = 2.0 * sqrt( 1.0 + v[0][0] - v[1][1] - v[2][2]); 222 | //q[0] = (v[1][2] - v[2][1] ) / s; 223 | q[0] = (v[2][1] - v[1][2] ) / s; 224 | q[1] = 0.25 * s; 225 | q[2] = (v[0][1] + v[1][0] ) / s; 226 | q[3] = (v[0][2] + v[2][0] ) / s; 227 | } 228 | else if (v[1][1] > v[2][2]) 229 | { 230 | double s = 2.0 * sqrt( 1.0 + v[1][1] - v[0][0] - v[2][2]); 231 | q[0] = (v[0][2] - v[2][0] ) / s; 232 | q[1] = (v[0][1] + v[1][0] ) / s; 233 | q[2] = 0.25 * s; 234 | q[3] = (v[1][2] + v[2][1] ) / s; 235 | } 236 | else 237 | { 238 | double s = 2.0 * sqrt( 1.0 + v[2][2] - v[0][0] - v[1][1] ); 239 | //q[0] = (v[0][1] - v[1][0] ) / s; 240 | q[0] = (v[1][0] - v[0][1]) / s; 241 | q[1] = (v[0][2] + v[2][0] ) / s; 242 | q[2] = (v[1][2] + v[2][1] ) / s; 243 | q[3] = 0.25 * s; 244 | } 245 | } 246 | return q; 247 | */ 248 | } 249 | 250 | RotMat RotMat::inv() const 251 | // Instead of using the standard inv() command from Mat, it computes the transpose 252 | { 253 | return(this->transp()); 254 | } 255 | -------------------------------------------------------------------------------- /abb_node/packages/matVec/RotMat.h: -------------------------------------------------------------------------------- 1 | #if !defined(ROTMAT_INCLUDED) 2 | #define ROTMAT_INCLUDED 3 | 4 | 5 | class Quaternion; 6 | 7 | class RotMat: public Mat 8 | { 9 | public: 10 | // Constructors 11 | RotMat(); 12 | RotMat(double const *values); 13 | RotMat(char const *string); 14 | RotMat(const Vec X, const Vec Y, const Vec Z); 15 | RotMat(const RotMat &original); // Copy constructor. 16 | RotMat(const Mat &original); // Conversion constructor. 17 | 18 | //Operators 19 | //(Explicitely Inherited for preserving the output class label) 20 | RotMat& operator=(const double constant); 21 | RotMat& operator+=(const RotMat &original); 22 | RotMat& operator-=(const RotMat &original); 23 | RotMat& operator*=(const double constant); 24 | RotMat& operator/=(const double constant); 25 | RotMat operator +(const RotMat &original) const; 26 | RotMat operator -(const RotMat &original) const; 27 | RotMat operator *(const RotMat &original) const; 28 | Vec operator *(const Vec &original) const; 29 | RotMat operator *(const double constant) const; 30 | RotMat operator /(const double constant) const; 31 | 32 | void setRefFrame(const Vec &X, const Vec &Y, const Vec &Z); 33 | 34 | void rotX(const double alfa); 35 | void rotY(const double alfa); 36 | void rotZ(const double alfa); 37 | void setAxisAngle(const Vec &vector, const double alfa); 38 | 39 | //Transformations 40 | double getAngle() const; 41 | Vec getAxis() const; 42 | Quaternion getQuaternion() const; 43 | 44 | RotMat inv() const; // Inverse computation redefinition. 45 | }; 46 | 47 | #endif // !defined(ROTMAT_INCLUDED) 48 | -------------------------------------------------------------------------------- /abb_node/packages/matVec/Vec.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "Vec.h" 6 | //#include "Mat.h" 7 | 8 | Vec::Vec() 9 | { 10 | nn=0; 11 | v=0; 12 | } 13 | 14 | Vec::Vec(const int n) 15 | { 16 | if(n>=0) 17 | { 18 | nn = n; 19 | v = new double[n]; 20 | } 21 | else{ 22 | Vec(); 23 | } 24 | } 25 | 26 | /** \brief Constructor. 27 | Initializes the vector to a constant value. 28 | * Constructor that initializes the vector to a constant value a. 29 | * @param constant initialization value. 30 | * @param n length of the vector. 31 | * @see Vec(const int n). 32 | */ 33 | Vec::Vec(const double constant, const int n) 34 | { 35 | if(n>=0){ 36 | nn = n; 37 | v = new double[n]; 38 | int i; 39 | for(i=0; i=0) 50 | { 51 | nn = n; 52 | v = new double[n]; 53 | int i; 54 | for(i=0; i0) 66 | { 67 | int i,j,k,error; 68 | char aux[30]; // Stores one of the floats in string. 69 | for (i=0;i<30;i++) 70 | aux[i]=0; 71 | 72 | nn = n; 73 | v = new double[n]; 74 | 75 | i=0; // Index of the char array string. 76 | j=0; // Index of the char array aux. 77 | k=0; // Index of vector. 78 | 79 | // Getting new elements of c until null-character. 80 | while (string[i]!=0) 81 | { 82 | if (string[i] != ' ') 83 | { 84 | // Add chars to the aux string until a space is found. 85 | aux[j]=string[i]; 86 | j++; 87 | } 88 | else 89 | { 90 | // Convert the string aux to a double, and put it into the matrix. 91 | error = sscanf(aux,"%lf",&v[k]); 92 | if (error > 0) 93 | { 94 | // Conversion ok. 95 | if (k==nn-1) 96 | { 97 | // More reals in string than length of vector vector. Vector filled. 98 | return; 99 | } 100 | // Go to next element of the matrix. 101 | k++; 102 | } 103 | // Reset aux. 104 | j=0; 105 | while ( (aux[j] != 0) && (j<30) ) 106 | { 107 | aux[j] = 0; 108 | j++; 109 | } 110 | j=0; 111 | } 112 | i++; // Next element of string. 113 | } 114 | if (string[i-1]!=0) 115 | { 116 | // Do the last conversion if necessary. 117 | sscanf(aux,"%lf",&v[k]); 118 | } 119 | } 120 | else Vec(); 121 | } 122 | 123 | Vec::Vec(const Vec &origin_vector) 124 | { 125 | nn = origin_vector.nn; 126 | v = new double[nn]; 127 | int i; 128 | for(i=0; i=0)&&(iaux) 355 | aux=*ve; 356 | ve++; 357 | } 358 | return aux; 359 | } 360 | 361 | double Vec::min() const 362 | { 363 | double aux=1e100; 364 | double *ve; 365 | ve=&v[0]; 366 | for (int i=0; iaux) 384 | { 385 | maxInd=i; 386 | aux=*ve; 387 | } 388 | ve++; 389 | } 390 | return maxInd; 391 | } 392 | 393 | int Vec::minInd() const 394 | { 395 | double aux=1e100; 396 | double *ve; 397 | ve=&v[0]; 398 | int minInd=0; 399 | for (int i=0; i 1) 458 | { 459 | // Swap a random unshuffled card with the top-most card 460 | k = rand() % n; 461 | n--; 462 | tmp = v[n]; 463 | v[n] = v[k]; 464 | v[k] = tmp; 465 | } 466 | } 467 | 468 | Vec Vec::abs() const 469 | { 470 | Vec w(nn); 471 | double *vpointer = &v[0]; 472 | double *wpointer = &w[0]; 473 | 474 | for (int i=0; i 12 | 13 | 14 | //#define SIGN(a, b) ((b) >= 0.0 ? fabs(a) : -fabs(a)) 15 | #define SIGN(a) (a > 0.0 ? 1 : (a<0.0 ? -1 : 0)) 16 | #define MAX(a,b) (a > b ? a:b) 17 | #define MIN(a,b) (a < b ? a:b) 18 | 19 | class Vec 20 | { 21 | public: 22 | int nn; // size of array. upper index is nn-1 23 | double *v; 24 | int error; 25 | 26 | //Constructors 27 | Vec(); 28 | Vec(const int n); // Zero-based array 29 | Vec(const double constant, const int n); //Initialize to constant value 30 | Vec(double const * values, const int n);// Initialize to values in C-style array a 31 | Vec(char const * string, const int n); //Initialize to values in string 32 | Vec(const Vec &origin_vector); // Copy constructor 33 | 34 | //Destructor 35 | ~Vec(); 36 | 37 | // Operators 38 | double & operator[](const int i) const; //i'th element 39 | Vec & operator = (const Vec &original); //assignment 40 | Vec & operator = (const double constant); //assign a to every element 41 | Vec & operator +=(const Vec &original); 42 | Vec & operator -=(const Vec &original); 43 | Vec & operator *=(const double constant); 44 | Vec & operator /=(const double constant); 45 | Vec operator +(const Vec &original) const; 46 | Vec operator -(const Vec &original) const; 47 | Vec operator -() const; 48 | double operator *(const Vec &original) const; // Dot product. 49 | Vec operator *(const double constant) const; 50 | Vec operator /(const double constant) const; 51 | Vec operator +(const double constant) const; 52 | Vec operator -(const double constant) const; 53 | Vec operator ^(const Vec &original) const; //Cross Product. Only for 3-vectors 54 | 55 | friend std::ostream& operator<<(std::ostream& os, const Vec &orig); 56 | 57 | 58 | //Linear Algebra 59 | double norm() const; 60 | void normalize(); 61 | double max() const; 62 | double min() const; 63 | int maxInd() const; 64 | int minInd() const; 65 | void randPerm(); 66 | Vec abs() const; 67 | 68 | //Statistics 69 | double mean() const; 70 | double variance() const; 71 | double stdev() const; 72 | }; 73 | 74 | #endif // !defined(VEC_INCLUDED) 75 | -------------------------------------------------------------------------------- /abb_node/packages/matVec/matVec.h: -------------------------------------------------------------------------------- 1 | #if !defined(PI_INCLUDED) 2 | #define PI 3.1415926535898 3 | #define TOLERANCE 0.0000000001 4 | #define DEG2RAD (PI / 180.0) 5 | #define RAD2DEG (180.0 / PI) 6 | #endif 7 | 8 | #if !defined(VEC_INCLUDED) 9 | #define VEC_INCLUDED 10 | 11 | #include 12 | 13 | 14 | //#define SIGN(a, b) ((b) >= 0.0 ? fabs(a) : -fabs(a)) 15 | #define SIGN(a) (a > 0.0 ? 1 : (a<0.0 ? -1 : 0)) 16 | #define MAX(a,b) (a > b ? a:b) 17 | #define MIN(a,b) (a < b ? a:b) 18 | 19 | class Vec 20 | { 21 | public: 22 | int nn; // size of array. upper index is nn-1 23 | double *v; 24 | int error; 25 | 26 | //Constructors 27 | Vec(); 28 | Vec(const int n); // Zero-based array 29 | Vec(const double constant, const int n); //Initialize to constant value 30 | Vec(double const * values, const int n);// Initialize to values in C-style array a 31 | Vec(char const * string, const int n); //Initialize to values in string 32 | Vec(const Vec &origin_vector); // Copy constructor 33 | 34 | //Destructor 35 | ~Vec(); 36 | 37 | // Operators 38 | double & operator[](const int i) const; //i'th element 39 | Vec & operator = (const Vec &original); //assignment 40 | Vec & operator = (const double constant); //assign a to every element 41 | Vec & operator +=(const Vec &original); 42 | Vec & operator -=(const Vec &original); 43 | Vec & operator *=(const double constant); 44 | Vec & operator /=(const double constant); 45 | Vec operator +(const Vec &original) const; 46 | Vec operator -(const Vec &original) const; 47 | Vec operator -() const; 48 | double operator *(const Vec &original) const; // Dot product. 49 | Vec operator *(const double constant) const; 50 | Vec operator /(const double constant) const; 51 | Vec operator +(const double constant) const; 52 | Vec operator -(const double constant) const; 53 | Vec operator ^(const Vec &original) const; //Cross Product. Only for 3-vectors 54 | 55 | friend std::ostream& operator<<(std::ostream& os, const Vec &orig); 56 | 57 | 58 | //Linear Algebra 59 | double norm() const; 60 | void normalize(); 61 | double max() const; 62 | double min() const; 63 | int maxInd() const; 64 | int minInd() const; 65 | void randPerm(); 66 | Vec abs() const; 67 | 68 | //Statistics 69 | double mean() const; 70 | double variance() const; 71 | double stdev() const; 72 | }; 73 | 74 | #endif // !defined(VEC_INCLUDED) 75 | #if !defined(MAT_INCLUDED) 76 | #define MAT_INCLUDED 77 | 78 | #include 79 | 80 | class Mat 81 | { 82 | public: 83 | int nn; ///< Number of rows. Index range is 0..nn-1. 84 | int mm; ///< Number of columns. Index range is 0..mm-1. 85 | double **v; ///< Storage of data. 86 | int error; //Error type. 87 | 88 | // Constructors. 89 | Mat(); 90 | Mat(const int n, const int m); 91 | Mat(const double constant, const int n, const int m); 92 | Mat(double const * values, const int n, const int m); 93 | Mat(char const * string, const int n, const int m); 94 | Mat(const Mat &origin_matrix); 95 | 96 | //Destructor 97 | ~Mat(); 98 | 99 | //Operators 100 | double* operator[](const int i) const; 101 | Mat& operator=(const Mat &original); 102 | Mat& operator=(const double constant); 103 | Mat& operator+=(const Mat &original); 104 | Mat& operator-=(const Mat &original); 105 | Mat operator +(const Mat &original) const; 106 | Mat operator -(const Mat &original) const; 107 | Mat operator *(const Mat &original) const; 108 | Vec operator *(const Vec &original) const; 109 | 110 | Mat operator -() const; 111 | 112 | Mat& operator+=(const double constant); 113 | Mat& operator-=(const double constant); 114 | Mat& operator*=(const double constant); 115 | Mat& operator/=(const double constant); 116 | Mat operator +(const double constant) const; 117 | Mat operator -(const double constant) const; 118 | Mat operator *(const double constant) const; 119 | Mat operator /(const double constant) const; 120 | 121 | friend std::ostream& operator<<(std::ostream& os, const Mat &orig); 122 | 123 | // LDU decomposition. 124 | Mat LDU(Vec &permutations, int &sign) const; 125 | void LDU(Mat &L, Mat &D, Mat &U, Mat &P) const; 126 | Vec LDUsolve(const Vec &b) const; 127 | Vec LDUsolve(const Mat &L, const Mat &D, const Mat &U, const Mat &P,const Vec &b) const; 128 | Mat LDUinverse() const; 129 | double LDUdet() const; 130 | Vec LSsolve(const Vec &b) const; 131 | 132 | //SVD decomposition. 133 | int SVD(Mat &U, Vec &sigma, Mat &V) const; 134 | 135 | //Linear Algebra 136 | Mat transp() const; 137 | Mat inv() const; 138 | double det() const; 139 | Vec getRow(const int n) const; 140 | Vec getCol(const int m) const; 141 | void setRow(const int n, const Vec &row); 142 | void setCol(const int m, const Vec &col); 143 | 144 | //Statistics 145 | double mean() const; 146 | double variance() const; 147 | double stdev() const; 148 | 149 | private: 150 | double PYTHAG(double a, double b) const; //For SVD decomposition 151 | 152 | }; 153 | 154 | #endif // !defined(MAT_INCLUDED) 155 | #if !defined(ROTMAT_INCLUDED) 156 | #define ROTMAT_INCLUDED 157 | 158 | 159 | class Quaternion; 160 | 161 | class RotMat: public Mat 162 | { 163 | public: 164 | // Constructors 165 | RotMat(); 166 | RotMat(double const *values); 167 | RotMat(char const *string); 168 | RotMat(const Vec X, const Vec Y, const Vec Z); 169 | RotMat(const RotMat &original); // Copy constructor. 170 | RotMat(const Mat &original); // Conversion constructor. 171 | 172 | //Operators 173 | //(Explicitely Inherited for preserving the output class label) 174 | RotMat& operator=(const double constant); 175 | RotMat& operator+=(const RotMat &original); 176 | RotMat& operator-=(const RotMat &original); 177 | RotMat& operator*=(const double constant); 178 | RotMat& operator/=(const double constant); 179 | RotMat operator +(const RotMat &original) const; 180 | RotMat operator -(const RotMat &original) const; 181 | RotMat operator *(const RotMat &original) const; 182 | Vec operator *(const Vec &original) const; 183 | RotMat operator *(const double constant) const; 184 | RotMat operator /(const double constant) const; 185 | 186 | void setRefFrame(const Vec &X, const Vec &Y, const Vec &Z); 187 | 188 | void rotX(const double alfa); 189 | void rotY(const double alfa); 190 | void rotZ(const double alfa); 191 | void setAxisAngle(const Vec &vector, const double alfa); 192 | 193 | //Transformations 194 | double getAngle() const; 195 | Vec getAxis() const; 196 | Quaternion getQuaternion() const; 197 | 198 | RotMat inv() const; // Inverse computation redefinition. 199 | }; 200 | 201 | #endif // !defined(ROTMAT_INCLUDED) 202 | #if !defined(QUATERNION_INCLUDED) 203 | #define QUATERNION_INCLUDED 204 | 205 | 206 | //class Vec; 207 | class RotMat; 208 | 209 | class Quaternion: public Vec 210 | { 211 | public: 212 | //Constructors 213 | Quaternion(); 214 | Quaternion(const double constant); //Initialize to constant value 215 | Quaternion(double const *values);// Initialize to values in C-style array a 216 | Quaternion(char const *string); //Initialize to values in string 217 | Quaternion(double const q0, Vec const v); //Initialize with scalar and vector 218 | Quaternion(const Vec &origin_vector); // Conversion constructor 219 | Quaternion(const Quaternion &origin_quaternion); //Copy constructor 220 | 221 | //Operators 222 | //(Explicitely Inherited for preserving the output class label) 223 | Quaternion & operator =(const double constant); //assign a to every element 224 | Quaternion & operator +=(const Quaternion &original); 225 | Quaternion & operator -=(const Quaternion &original); 226 | Quaternion operator +(const Quaternion &original) const; 227 | Quaternion operator -(const Quaternion &original) const; 228 | Quaternion operator -() const; 229 | Quaternion operator *(const double constant) const; 230 | Quaternion operator /(const double constant) const; 231 | //(New) 232 | Quaternion operator ^(const Quaternion &original) const; // Quaternion product. 233 | double operator *(const Quaternion &original) const; 234 | 235 | //Quaternion Algebra 236 | Quaternion conjugate() const; 237 | Quaternion inverse() const; 238 | double getScalar() const; 239 | Vec getVector() const; 240 | void setScalar(const double s); 241 | void setVector(const Vec &vector); 242 | Mat leftMat() const; 243 | Mat rightMat() const; 244 | 245 | //Transformation 246 | double getAngle() const; 247 | Vec getAxis() const; 248 | RotMat getRotMat() const; 249 | }; 250 | 251 | #endif // !defined(QUATERNION_INCLUDED) 252 | #if !defined(HOMOGTRANSF_INCLUDED) 253 | #define HOMOGTRANSF_INCLUDED 254 | 255 | class HomogTransf : public Mat 256 | { 257 | public: 258 | //Constructors 259 | HomogTransf(); // Null translation and rotation. 260 | HomogTransf(double const *values); 261 | HomogTransf(char const *string); 262 | HomogTransf(const HomogTransf &original); // Copy constructor. 263 | HomogTransf(const RotMat &rot, const Vec &trans); 264 | HomogTransf(const Mat &original); // Conversion constructor. 265 | 266 | //Operators 267 | //(Explicitely Inherited for preserving the output class label) 268 | HomogTransf& operator=(const double constant); 269 | HomogTransf& operator+=(const HomogTransf &original); 270 | HomogTransf& operator-=(const HomogTransf &original); 271 | HomogTransf& operator*=(const double constant); 272 | HomogTransf& operator/=(const double constant); 273 | HomogTransf operator +(const HomogTransf &original) const; 274 | HomogTransf operator -(const HomogTransf &original) const; 275 | HomogTransf operator *(const HomogTransf &original) const; 276 | HomogTransf operator *(const double constant) const; 277 | HomogTransf operator /(const double constant) const; 278 | //New Operator 279 | Vec operator *(const Vec &original) const; //If original is a 4-vector: Standard product. 280 | //If original is a 3-vector: Transformation of a point in R3. 281 | //Interface 282 | void setTranslation(const Vec &trans); 283 | void setRotation(const RotMat &rot); 284 | RotMat getRotation() const; 285 | Vec getTranslation() const; 286 | 287 | //Transformations 288 | void setScrew(const Vec &point,const Vec &director_vector,const double displacement,const double angle); 289 | 290 | //Algebra 291 | HomogTransf inv() const; 292 | 293 | }; 294 | 295 | #endif // !defined(HOMOGTRANSF_INCLUDED) 296 | #if !defined(POLYNOM_INCLUDED) 297 | #define POLYNOM_INCLUDED 298 | 299 | class Polynom : public Vec 300 | { 301 | public: 302 | 303 | //Constructors 304 | Polynom(); 305 | Polynom(const int n); // Zero-based array 306 | Polynom(const double constant, const int n); //Initialize to constant value 307 | Polynom(double const *values, const int n);// Initialize to values in C-style array a 308 | Polynom(char const *string, const int n); //Initialize to values in string 309 | Polynom(const Vec &origin_vector); // Copy constructor 310 | Polynom(const Polynom &origin_polynom); //Copy constructor 311 | 312 | //Operators 313 | //(Explicitely Inherited for preserving the output class label) 314 | Polynom & operator =(const double constant); //assign a to every element 315 | Polynom operator -() const; 316 | Polynom operator *(const double constant); 317 | Polynom operator /(const double constant); 318 | //(New or redefined) 319 | Polynom operator *(const Polynom &original); // Polynom product. 320 | Polynom & operator +=(const Polynom &original); 321 | Polynom & operator -=(const Polynom &original); 322 | Polynom operator +(const Polynom &original); //Polynom Sum. 323 | Polynom operator -(const Polynom &original); //Polynom Sum. 324 | double operator()(const double x); //Evaluate Polynomial. 325 | 326 | int degree() const; 327 | void interpolate(const Vec &x, const Vec &y, const int n); 328 | }; 329 | 330 | #endif // !defined(POLYNOM_INCLUDED) 331 | -------------------------------------------------------------------------------- /abb_node/src/abb_node.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "abb_comm.h" 13 | #include "matVec.h" 14 | 15 | //ROS specific 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | //ROS specific, these are redundant with abb_node 36 | //standard libary messages instead of custom messages 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | //#define MAX_BUFFER 256 43 | #define MAX_BUFFER 10000 44 | #define ID_CODE_MAX 999 45 | 46 | #define SERVER_BAD_MSG 0 47 | #define SERVER_OK 1 48 | #define SERVER_COLLISION 2 49 | 50 | #define MAX_TRANS_STEP 2.0 51 | #define MAX_ROT_STEP (0.5 * DEG2RAD) 52 | #define MAX_J_STEP 0.5 53 | 54 | #define NB_FREQ 200.0 55 | #define STOP_CHECK_FREQ 25.0 56 | #define DIST_CHECK_FREQ 100.0 57 | 58 | #define SAFETY_FACTOR 0.90 59 | #define MINIMUM_TRACK_DIST_TRANS 1.0 //mm 60 | #define MAXIMUM_TRACK_DIST_TRANS 20.0 //mm 61 | #define MINIMUM_TRACK_DIST_ORI 0.333 //deg 62 | #define MAXIMUM_TRACK_DIST_ORI 6.66 //deg 63 | #define INFINITY_TRACK_DIST_TRANS 1000.0 ///mm 64 | #define INFINITY_TRACK_DIST_ORI 333.0 //deg 65 | 66 | #define MINIMUM_NB_SPEED_TCP 1.0 //mm/s 67 | #define MINIMUM_NB_SPEED_ORI 0.333 //deg/s 68 | 69 | 70 | #define NUM_JOINTS 6 71 | #define NUM_FORCES 6 72 | 73 | #define BLOCKING 1 74 | #define NON_BLOCKING 0 75 | 76 | #define VACUUM_OPEN 0 77 | #define VACUUM_CLOSE 1 78 | 79 | typedef enum 80 | { 81 | ZONE_FINE = 0, 82 | ZONE_1, 83 | ZONE_2, 84 | ZONE_3, 85 | ZONE_4, 86 | ZONE_5, 87 | NUM_ZONES 88 | } ZONE_TYPE; 89 | 90 | typedef struct 91 | { 92 | double p_tcp; // TCP path zone (mm) 93 | double p_ori; // Zone size for orientation (mm) 94 | double ori; // Tool orientation (degrees) 95 | } zone_vals; 96 | 97 | static const zone_vals zone_data[NUM_ZONES] = 98 | { 99 | // p_tcp (mm), p_ori (mm), ori (deg) 100 | {0.0, 0.0, 0.0}, // ZONE_FINE 101 | {0.3, 0.3, 0.03}, // ZONE_1 102 | {1.0, 1.0, 0.1}, // ZONE_2 103 | {5.0, 8.0, 0.8}, // ZONE_3 104 | {10.0, 15.0, 1.5}, // ZONE_4 105 | {20.0, 30.0, 3.0} // ZONE_5 106 | }; 107 | 108 | // Mutex used for threads 109 | pthread_mutex_t nonBlockMutex; 110 | pthread_mutex_t jointUpdateMutex; 111 | pthread_mutex_t cartUpdateMutex; 112 | pthread_mutex_t wobjUpdateMutex; 113 | pthread_mutex_t forceUpdateMutex; 114 | pthread_mutex_t sendRecvMutex; 115 | 116 | class RobotController 117 | { 118 | public: 119 | RobotController(ros::NodeHandle * n); 120 | virtual ~RobotController(); 121 | 122 | // Initialize the robot 123 | bool init(); 124 | 125 | // Service Callbacks 126 | bool robot_Ping( 127 | abb_node::robot_Ping::Request& req, 128 | abb_node::robot_Ping::Response& res); 129 | bool robot_SetCartesian( 130 | abb_node::robot_SetCartesian::Request& req, 131 | abb_node::robot_SetCartesian::Response& res); 132 | bool robot_GetCartesian( 133 | abb_node::robot_GetCartesian::Request& req, 134 | abb_node::robot_GetCartesian::Response& res); 135 | bool robot_SetJoints( 136 | abb_node::robot_SetJoints::Request& req, 137 | abb_node::robot_SetJoints::Response& res); 138 | bool robot_GetJoints( 139 | abb_node::robot_GetJoints::Request& req, 140 | abb_node::robot_GetJoints::Response& res); 141 | bool robot_Stop( 142 | abb_node::robot_Stop::Request& req, 143 | abb_node::robot_Stop::Response& res); 144 | bool robot_SetTool( 145 | abb_node::robot_SetTool::Request& req, 146 | abb_node::robot_SetTool::Response& res); 147 | bool robot_SetWorkObject( 148 | abb_node::robot_SetWorkObject::Request& req, 149 | abb_node::robot_SetWorkObject::Response& res); 150 | bool robot_SetComm( 151 | abb_node::robot_SetComm::Request& req, 152 | abb_node::robot_SetComm::Response& res); 153 | bool robot_SpecialCommand( 154 | abb_node::robot_SpecialCommand::Request& req, 155 | abb_node::robot_SpecialCommand::Response& res); 156 | bool robot_SetVacuum( 157 | abb_node::robot_SetVacuum::Request& req, 158 | abb_node::robot_SetVacuum::Response& res); 159 | bool robot_SetDIO( 160 | abb_node::robot_SetDIO::Request& req, 161 | abb_node::robot_SetDIO::Response& res); 162 | bool robot_SetSpeed( 163 | abb_node::robot_SetSpeed::Request& req, 164 | abb_node::robot_SetSpeed::Response& res); 165 | bool robot_SetZone( 166 | abb_node::robot_SetZone::Request& req, 167 | abb_node::robot_SetZone::Response& res); 168 | bool robot_SetTrackDist( 169 | abb_node::robot_SetTrackDist::Request& req, 170 | abb_node::robot_SetTrackDist::Response& res); 171 | bool robot_IsMoving( 172 | abb_node::robot_IsMoving::Request& req, 173 | abb_node::robot_IsMoving::Response& res); 174 | 175 | 176 | // Advertise Services and Topics 177 | void advertiseServices(); 178 | void advertiseTopics(); 179 | 180 | // Call back function for the logging which will be called by a timer event 181 | void logCallback(const ros::TimerEvent&); 182 | 183 | // Public access to the ROS node 184 | ros::NodeHandle *node; 185 | 186 | // Non-Blocking move variables 187 | bool non_blocking; // Whether we are in non-blocking mode 188 | bool do_nb_move; // Whether we are currently moving in non-blocking mode 189 | bool targetChanged; // Whether a new target was specified 190 | bool stopRequest; // Set to true when we are trying to stop the robot 191 | bool stopConfirm; // Set to true when the thread is sure it's stopped 192 | bool cart_move; // True if we're doing a cartesian move, false if joint 193 | 194 | // Variables dealing with changing non-blocking speed and step sizes 195 | bool changing_nb_speed; // Overrides setSpeed safety 196 | double curCartStep; // Largest cartesian stepsize during non-blocking 197 | double curOrientStep; // Largest orientation step size during non-blocking 198 | double curJointStep; // Largest joint step size during non-blocking 199 | double curDist[3]; // Max allowable tracking error (pos, ang, joint) 200 | 201 | // Most recent goal position, and the final target position 202 | Vec curGoalP; 203 | Quaternion curGoalQ; 204 | Vec curTargP; 205 | Quaternion curTargQ; 206 | double curGoalJ[NUM_JOINTS]; 207 | double curTargJ[NUM_JOINTS]; 208 | 209 | // Move commands are public so that the non-blocking thread can use it 210 | bool setCartesian(double x, double y, double z, 211 | double q0, double qx, double qy, double qz); 212 | bool setJoints(double position[]); 213 | 214 | // Functions that compute our distance from the current position to the goal 215 | double posDistFromGoal(); 216 | double orientDistFromGoal(); 217 | double jointDistFromGoal(); 218 | 219 | private: 220 | // Socket Variables 221 | bool motionConnected; 222 | bool loggerConnected; 223 | int robotMotionSocket; 224 | int robotLoggerSocket; 225 | 226 | // Connect to servers on the robot 227 | bool connectMotionServer(const char* ip, int port); 228 | bool connectLoggerServer(const char* ip, int port); 229 | 230 | // Sets up the default robot configuration 231 | bool defaultRobotConfiguration(); 232 | 233 | //handles to ROS stuff 234 | tf::TransformBroadcaster handle_tf; 235 | //Duplicates with standard messages 236 | ros::Publisher handle_JointsLog; 237 | ros::Publisher handle_ForceLog; 238 | ros::Publisher handle_CartesianLog; 239 | 240 | ros::ServiceServer handle_Ping; 241 | ros::ServiceServer handle_SetCartesian; 242 | ros::ServiceServer handle_GetCartesian; 243 | ros::ServiceServer handle_SetJoints; 244 | ros::ServiceServer handle_GetJoints; 245 | ros::ServiceServer handle_Stop; 246 | ros::ServiceServer handle_SetTool; 247 | ros::ServiceServer handle_SetWorkObject; 248 | ros::ServiceServer handle_SetSpeed; 249 | ros::ServiceServer handle_SetZone; 250 | ros::ServiceServer handle_SetTrackDist; 251 | ros::ServiceServer handle_SpecialCommand; 252 | ros::ServiceServer handle_SetComm; 253 | ros::ServiceServer handle_SetVacuum; 254 | ros::ServiceServer handle_SetDIO; 255 | ros::ServiceServer handle_IsMoving; 256 | 257 | // Helper function for communicating with robot server 258 | bool sendAndReceive(char *message, int messageLength, 259 | char*reply, int idCode=-1); 260 | 261 | // Internal functions that communicate with the robot 262 | bool ping(); 263 | bool getCartesian(double &x, double &y, double &z, 264 | double &q0, double &qx, double &qy, double &qz); 265 | bool getJoints(double &j1, double &j2, double &j3, 266 | double &j4, double &j5, double &j6); 267 | bool setTool(double x, double y, double z, 268 | double q0, double qx, double qy, double qz); 269 | bool setWorkObject(double x, double y, double z, 270 | double q0, double qx, double qy, double qz); 271 | bool specialCommand(int command, double param1=0, double param2=0, double param3=0, double param4=0, double param5=0); 272 | bool setVacuum(int v); 273 | bool setDIO(int dio_num, int dio_state); 274 | bool setSpeed(double tcp, double ori); 275 | bool setZone(int z); 276 | bool stop_nb(); 277 | 278 | // Check if robot is currently moving or not 279 | bool is_moving(); 280 | 281 | // Functions to handle setting up non-blocking step sizes 282 | bool setTrackDist(double pos_dist, double ang_dist); 283 | bool setNonBlockSpeed(double tcp, double ori); 284 | 285 | // Robot State 286 | double curSpd[2]; 287 | int curVacuum; 288 | int curZone; 289 | Vec curToolP; 290 | Quaternion curToolQ; 291 | Vec curWorkP; 292 | Quaternion curWorkQ; 293 | tf::Transform curWobjTransform; 294 | 295 | 296 | // Robot Position and Force Information 297 | Vec curP; 298 | Quaternion curQ; 299 | double curJ[NUM_JOINTS]; 300 | double curForce[NUM_FORCES]; 301 | }; 302 | -------------------------------------------------------------------------------- /abb_node/srv/robot_GetCartesian.srv: -------------------------------------------------------------------------------- 1 | # Service to get the cartesian position of the robot 2 | 3 | --- 4 | float64 x 5 | float64 y 6 | float64 z 7 | float64 q0 8 | float64 qx 9 | float64 qy 10 | float64 qz 11 | int64 ret 12 | string msg -------------------------------------------------------------------------------- /abb_node/srv/robot_GetJoints.srv: -------------------------------------------------------------------------------- 1 | # Service to get joint angles of the robot 2 | 3 | --- 4 | float64 j1 5 | float64 j2 6 | float64 j3 7 | float64 j4 8 | float64 j5 9 | float64 j6 10 | int64 ret 11 | string msg -------------------------------------------------------------------------------- /abb_node/srv/robot_IsMoving.srv: -------------------------------------------------------------------------------- 1 | # Service to get whether robot is moving or not 2 | 3 | --- 4 | bool moving 5 | int64 ret 6 | string msg 7 | -------------------------------------------------------------------------------- /abb_node/srv/robot_Ping.srv: -------------------------------------------------------------------------------- 1 | #Service to Ping the robot 2 | 3 | --- 4 | int64 ret 5 | string msg -------------------------------------------------------------------------------- /abb_node/srv/robot_SetCartesian.srv: -------------------------------------------------------------------------------- 1 | # Service to Set Cartesians 2 | float64[] cartesian 3 | float64[] quaternion 4 | --- 5 | int64 ret 6 | string msg -------------------------------------------------------------------------------- /abb_node/srv/robot_SetComm.srv: -------------------------------------------------------------------------------- 1 | #Service to set the communication mode of the robot 2 | 3 | int64 mode #1-Blocking; 0-Nonblocking 4 | --- 5 | int64 ret 6 | string msg -------------------------------------------------------------------------------- /abb_node/srv/robot_SetDIO.srv: -------------------------------------------------------------------------------- 1 | #Service to set digital outputs on/off 2 | int64 DIO_num #digital IO number 3 | bool state #1-on; 0-off 4 | --- 5 | int64 ret 6 | string msg -------------------------------------------------------------------------------- /abb_node/srv/robot_SetJoints.srv: -------------------------------------------------------------------------------- 1 | # Service to Set Joints 2 | float64[] position 3 | 4 | --- 5 | int64 ret 6 | string msg -------------------------------------------------------------------------------- /abb_node/srv/robot_SetSpeed.srv: -------------------------------------------------------------------------------- 1 | # Service to Set the max Speed of the robot. 2 | 3 | float64 tcp # mm/s 4 | float64 ori # deg/s 5 | --- 6 | int64 ret 7 | string msg 8 | -------------------------------------------------------------------------------- /abb_node/srv/robot_SetTool.srv: -------------------------------------------------------------------------------- 1 | # Service to Set the Tool of the robot in cartesian coordinates 2 | 3 | float64 x 4 | float64 y 5 | float64 z 6 | float64 q0 7 | float64 qx 8 | float64 qy 9 | float64 qz 10 | --- 11 | int64 ret 12 | string msg 13 | -------------------------------------------------------------------------------- /abb_node/srv/robot_SetTrackDist.srv: -------------------------------------------------------------------------------- 1 | # Service to Set the tracking distance of the robot while in non-blocking mode 2 | 3 | float64 pos_dist # mm 4 | float64 ang_dist # deg 5 | --- 6 | int64 ret 7 | string msg 8 | -------------------------------------------------------------------------------- /abb_node/srv/robot_SetVacuum.srv: -------------------------------------------------------------------------------- 1 | #Service to set vacuum on/off 2 | 3 | int64 vacuum #1-on; 0-off 4 | --- 5 | int64 ret 6 | string msg -------------------------------------------------------------------------------- /abb_node/srv/robot_SetWorkObject.srv: -------------------------------------------------------------------------------- 1 | # Service to Set the Work Object of the robot 2 | 3 | float64 x 4 | float64 y 5 | float64 z 6 | float64 q0 7 | float64 qx 8 | float64 qy 9 | float64 qz 10 | --- 11 | int64 ret 12 | string msg 13 | -------------------------------------------------------------------------------- /abb_node/srv/robot_SetZone.srv: -------------------------------------------------------------------------------- 1 | # Service to Set the Zone of the robot 2 | # Mode - Name in RAPID - Linear - Orientation 3 | # 0 fine 0 mm 0° 4 | # 1 z0 0.3 mm 0.03° <- Default and recommended value. 5 | # 2 z1 1 mm 0.1° 6 | # 3 z5 5 mm 0.8° 7 | # 4 z10 10 mm 1.5° 8 | 9 | int64 mode 10 | --- 11 | int64 ret 12 | string msg 13 | -------------------------------------------------------------------------------- /abb_node/srv/robot_SpecialCommand.srv: -------------------------------------------------------------------------------- 1 | #Service to run spcial command 2 | 3 | int64 command #integer identificating the command to run 4 | float64 param1 #Special purpose parameter number 1 5 | float64 param2 #Special purpose parameter number 2 6 | float64 param3 #Special purpose parameter number 3 7 | float64 param4 #Special purpose parameter number 4 8 | float64 param5 #Special purpose parameter number 5 9 | --- 10 | int64 ret 11 | string msg 12 | -------------------------------------------------------------------------------- /abb_node/srv/robot_Stop.srv: -------------------------------------------------------------------------------- 1 | #Service to Stop the robot 2 | 3 | --- 4 | int64 ret 5 | string msg --------------------------------------------------------------------------------