Problem working with real IRB 1200 robot about abb HOT 17 CLOSED

18c18
<   <arg name="robot_ip" doc="IP of the controller"/>
---
>   <arg name="robot_ip" />
21c21
<   <arg name="J23_coupled" default="false" doc="If true, compensate for J2-J3 parallel linkage" />
---
>   <arg name="J23_coupled" default="false" />
diff -Nr abb/abb_driver/package.xml ABB-IRB-1200/abb_driver/package.xml
16,17c16,17
<   <author>Shaun Edwards</author>
<   <maintainer email="[email protected]">Levi Armstrong (Southwest Research Institute)</maintainer>
---
>   <author email="[email protected]">Shaun Edwards</author>
>   <maintainer email="[email protected]">Shaun Edwards</maintainer>
diff -Nr abb/abb_driver/rapid/ROS_common.sys ABB-IRB-1200/abb_driver/rapid/ROS_common.sys
33d32
<     extjoint extax_pos;
diff -Nr abb/abb_driver/rapid/ROS_messages.sys ABB-IRB-1200/abb_driver/rapid/ROS_messages.sys
44d43
<     extjoint ext_axes;
53d51
<     extjoint ext_axes;
112,118c110,113
<     UnpackRawBytes raw_message.data, 29, message.ext_axes.eax_a, \Float4;
<     UnpackRawBytes raw_message.data, 33, message.ext_axes.eax_b, \Float4;
<     UnpackRawBytes raw_message.data, 37, message.ext_axes.eax_c, \Float4;
<     UnpackRawBytes raw_message.data, 41, message.ext_axes.eax_d, \Float4;
<     UnpackRawBytes raw_message.data, 45, message.velocity, \Float4;
<     UnpackRawBytes raw_message.data, 49, message.duration, \Float4;
<
---
>     ! Skip bytes 29-44.  UNUSED.  Reserved for Joints 7-10.  TBD: copy to extAx?
>     UnpackRawBytes raw_message.data, 29+(ROS_MSG_MAX_JOINTS-6)*4, message.velocity, \Float4;
>     UnpackRawBytes raw_message.data, 33+(ROS_MSG_MAX_JOINTS-6)*4, message.duration, \Float4;
>
121d115
<     message.ext_axes := m2mm_extjoint(message.ext_axes);
131d124
<     VAR extjoint ROS_ext_axes;
141d133
<     ROS_ext_axes := mm2m_extjoint(message.ext_axes);
151,154c143,145
<     PackRawBytes ROS_ext_axes.eax_a,  raw_message.data, 29, \Float4;
<     PackRawBytes ROS_ext_axes.eax_b,  raw_message.data, 33, \Float4;
<     PackRawBytes ROS_ext_axes.eax_c,  raw_message.data, 37, \Float4;
<     PackRawBytes ROS_ext_axes.eax_d,  raw_message.data, 41, \Float4;
---
>     FOR i FROM 1 TO ROS_MSG_MAX_JOINTS-6 DO   ! Insert placeholders for joints 7-10 (message expects 10 joints)
>         PackRawBytes 0,               raw_message.data, 25+i*4, \Float4;
>     ENDFOR
164,192d154
< ENDFUNC
<
< LOCAL FUNC num connected_eax(num eax)
<     ! The value 9E9 is defined for axes which are not connected
<     IF eax <> 9E9 THEN
<         RETURN eax;
<     ENDIF
<     RETURN 0;
< ENDFUNC
<
< ! Returns only connected external axes in METERS
< LOCAL FUNC extjoint mm2m_extjoint(extjoint all_eax)
<     VAR extjoint eax;
<     eax.eax_a := connected_eax(all_eax.eax_a) / 1000;
<     eax.eax_b := connected_eax(all_eax.eax_b) / 1000;
<     eax.eax_c := connected_eax(all_eax.eax_c) / 1000;
<     eax.eax_d := connected_eax(all_eax.eax_d) / 1000;
<     RETURN eax;
< ENDFUNC
<
< ! Returns external axes in MILLIMETERS
< LOCAL FUNC extjoint m2mm_extjoint(extjoint eax_in_m)
<     VAR extjoint eax_in_mm;
<     eax_in_mm.eax_a := eax_in_m.eax_a * 1000;
<     eax_in_mm.eax_b := eax_in_m.eax_b * 1000;
<     eax_in_mm.eax_c := eax_in_m.eax_c * 1000;
<     eax_in_mm.eax_d := eax_in_m.eax_d * 1000;
<
<     RETURN eax_in_mm;
diff -Nr abb/abb_driver/rapid/ROS_motion.mod ABB-IRB-1200/abb_driver/rapid/ROS_motion.mod
57d56
<                 target.extax := trajectory{current_index}.extax_pos;
59c58
<                 skip_move := (current_index = 1) AND is_near(target, 0.1, 0.1);
---
>                 skip_move := (current_index = 1) AND is_near(target.robax, 0.1);
89c88
< LOCAL FUNC bool is_near(jointtarget target, num deg_tol, num mm_tol)
---
> LOCAL FUNC bool is_near(robjoint target, num tol)
94,103c93,98
<     RETURN ( ABS(curr_jnt.robax.rax_1 - target.robax.rax_1) < deg_tol )
<        AND ( ABS(curr_jnt.robax.rax_2 - target.robax.rax_2) < deg_tol )
<        AND ( ABS(curr_jnt.robax.rax_3 - target.robax.rax_3) < deg_tol )
<        AND ( ABS(curr_jnt.robax.rax_4 - target.robax.rax_4) < deg_tol )
<        AND ( ABS(curr_jnt.robax.rax_5 - target.robax.rax_5) < deg_tol )
<        AND ( ABS(curr_jnt.robax.rax_6 - target.robax.rax_6) < deg_tol )
<        AND ( ABS(curr_jnt.extax.eax_a - target.extax.eax_a) < mm_tol )
<        AND ( ABS(curr_jnt.extax.eax_b - target.extax.eax_b) < mm_tol )
<        AND ( ABS(curr_jnt.extax.eax_c - target.extax.eax_c) < mm_tol )
<        AND ( ABS(curr_jnt.extax.eax_d - target.extax.eax_d) < mm_tol );
---
>     RETURN ( ABS(curr_jnt.robax.rax_1 - target.rax_1) < tol )
>        AND ( ABS(curr_jnt.robax.rax_2 - target.rax_2) < tol )
>        AND ( ABS(curr_jnt.robax.rax_3 - target.rax_3) < tol )
>        AND ( ABS(curr_jnt.robax.rax_4 - target.rax_4) < tol )
>        AND ( ABS(curr_jnt.robax.rax_5 - target.rax_5) < tol )
>        AND ( ABS(curr_jnt.robax.rax_6 - target.rax_6) < tol );
diff -Nr abb/abb_driver/rapid/ROS_motionServer.mod ABB-IRB-1200/abb_driver/rapid/ROS_motionServer.mod
69c69
<     point := [message.joints, message.ext_axes, message.duration];
---
>     point := [message.joints, message.duration];
diff -Nr abb/abb_driver/rapid/ROS_socket.sys ABB-IRB-1200/abb_driver/rapid/ROS_socket.sys
32c32
<     IF (SocketGetStatus(server_socket) = SOCKET_CREATED) SocketBind server_socket, GetSysInfo(\LanIp), port;
---
>     IF (SocketGetStatus(server_socket) = SOCKET_CREATED) SocketBind server_socket, 192.168.125.5, port;
104c104
< ENDMODULE
\ No newline at end of file
---
> ENDMODULE
diff -Nr abb/abb_driver/rapid/ROS_stateServer.mod ABB-IRB-1200/abb_driver/rapid/ROS_stateServer.mod
70d69
<     message.ext_axes := joints.extax;