Merge pull request #19 from norkator/MOVJ

MOVJ support

README.md

@@ -15,6 +15,7 @@ Table of contents
 * [Lint, Test, Build](#lint-test-build)
 * [Programs](#programs)
 * [MoveL](#movel)
+* [MoveJ](#movej)
 * [Arduino gripper](#arduino-gripper)
     * [Ladder changes](#ladder-changes)
     * [Python sample](#python-sample)
@@ -95,8 +96,7 @@ Build: `python -m build`
 Programs
 ============
 
-`Testing.py` => Used for development and testing individual commands.  
-`WebServer.py` => Hosts small web page having control possibilities (repository readme photo)  
+`Testing.py` => Used for development and testing individual commands.   
 `XboxController.py` => As name says, can use controller to control robot, just demo.  
 
 
@@ -171,6 +171,34 @@ linear_move.go(move_l=move_l, wait=True, poll_limit_seconds=10)
 print('finished')
 ```
 
+
+MoveJ
+============
+Quick sample for MovJ command to do joint motion movement with robot. 
+Read `Ethernet Server Function Manual` for more details about MOVJ
+
+<font color="red">!!! BE CAREFUL WITH MOVJ COMMAND AND IT'S SPEED SETTING !!!</font>
+* Speed is given as percentage from 1 to 100.
+* Start running it with lower speed. 
+
+```python
+from nx100_remote_control.module import JointMove, Utils
+from nx100_remote_control.objects import MoveJ
+
+move_j = MoveJ.MoveJ(
+    25,  # speed %
+    MoveJ.MoveJ.coordinate_specification_base_coordinate,
+    352.769, 202.779, 120.658,
+    -1.34, 35.78, 27.84,
+    Utils.binary_to_decimal(0x00000001),
+    0, 0, 0, 0, 0, 0, 0
+)
+linear_move = JointMove.JointMove()
+linear_move.go(move_j=move_j, wait=True, poll_limit_seconds=10)
+print('finished')
+```
+
+
 Arduino gripper
 ============
 Arduino folder contains code and sketch for custom Gripper integrated for NX100 Motoman.

Testing.py

@@ -3,7 +3,9 @@
 """
 
 import nx100_remote_control
-from nx100_remote_control.module import Commands, WebServer
+from nx100_remote_control.module import Commands, WebServer, Utils, JointMove
+from nx100_remote_control.objects import MoveJ
+from nx100_remote_control.objects import MoveJ
 import logging
 import os
 
@@ -22,7 +24,7 @@ def start_app():
     nx100_remote_control.MOCK_RESPONSE = True
     # WebServer.run(addr="localhost", port=8080)
 
-    Commands.read_alarms()
+    # Commands.read_alarms()
     # Commands.read_current_joint_coordinate_position()
     # Commands.read_current_specified_coordinate_system_position('0', '0')
     # Commands.read_status()
@@ -81,5 +83,16 @@ def start_app():
     # linear_move.go(move_l=move_l, wait=True, poll_limit_seconds=10)
     # print('finished')
 
+    move_j = MoveJ.MoveJ(
+        25,  # speed %
+        MoveJ.MoveJ.coordinate_specification_base_coordinate,
+        352.769, 202.779, 120.658,
+        -1.34, 35.78, 27.84,
+        Utils.binary_to_decimal(0x00000001),
+        0, 0, 0, 0, 0, 0, 0
+    )
+    linear_move = JointMove.JointMove()
+    linear_move.go(move_j=move_j, wait=True, poll_limit_seconds=10)
+
 
 start_app()

src/nx100_remote_control/module/Commands.py

@@ -122,6 +122,17 @@ def write_linear_move(move_l):
     return response
 
 
+# Moves a manipulator to a specified coordinate position in joint motion.
+def write_joint_motion_move(move_j):
+    move_cmd = move_j.get_command()
+    logging.info('[i] move: ' + move_cmd)
+    response = Response.Response(Socket.exec_single_command(
+        Command.Command("MOVJ", move_cmd)
+    ))
+    logging.info('[i] command run successfully!' if response.is_success() else '[E] command run failed!')
+    return response
+
+
 # Is robot in target point function with callback. Timeout given in seconds
 def robot_in_target_point_callback(move_l, timeout=10, _callback_success=None, _callback_failed=None):
     current = 0

src/nx100_remote_control/module/JointMove.py

@@ -0,0 +1,49 @@
+"""
+Commander class for joint moves
+"""
+
+from nx100_remote_control.module import Commands, Utils
+import time
+
+
+class JointMove(object):
+
+    # Class constructor
+    def __init__(self):
+        self.stopped = False
+
+    def go(self, move_j, wait=True, poll_limit_seconds=30):
+        """
+        commands robot to move into position linear way
+
+        :param move_j: MoveJ object describing target position and move settings
+        :param wait: wait for move to complete or not
+        :param poll_limit_seconds: functions as a timeout of wait is enabled
+        :return: boolean
+        """
+        self.stopped = False
+        Commands.write_hold('0')  # disable hold if currently enabled
+        Commands.write_joint_motion_move(move_j=move_j)  # execute wanted move command
+        current = 0
+        if not wait:
+            return True
+        for x in range(poll_limit_seconds):
+            if self.stopped:
+                return False
+            time.sleep(1)
+            cp = Commands.read_current_specified_coordinate_system_position(  # returns CurrentPos object
+                str(move_j.get_coordinate_specification), '0'
+            )
+            if Utils.is_in_position(move_j, cp):
+                return True
+            else:
+                current = current + 1
+                if current == poll_limit_seconds:
+                    return False
+
+    def stop(self):
+        """
+        stop upper go function
+        """
+        self.stopped = True
+        Commands.write_hold('1')  # only way to stop robot from executing move

src/nx100_remote_control/module/LinearMove.py

@@ -23,6 +23,7 @@ def go(self, move_l, wait=True, poll_limit_seconds=30):
         """
         self.stopped = False
         Commands.write_hold('0')  # disable hold if currently enabled
+        Commands.write_linear_move(move_l=move_l)  # execute wanted move command
         current = 0
         if not wait:
             return True

src/nx100_remote_control/module/MockResponse.py

@@ -36,6 +36,9 @@
 # noinspection SpellCheckingInspection
 MOVL = "b'0000\\r\\n'"
 
+# noinspection SpellCheckingInspection
+MOVJ = "b'0000\\r\\n'"
+
 # noinspection SpellCheckingInspection
 IOWRITE = "b'0\\r\\n'"
 
@@ -66,6 +69,8 @@ def get_mock_response(command):
         return START
     elif command.name == 'MOVL':
         return MOVL
+    elif command.name == 'MOVJ':
+        return MOVJ
     elif command.name == 'IOWRITE':
         return IOWRITE
     elif command.name == 'IOREAD':

src/nx100_remote_control/objects/MoveJ.py

@@ -0,0 +1,74 @@
+class MoveJ(object):
+
+    """
+    !!! WARNING !!!
+    !!! motion_speed_percentage 100% could be dangerous !!!
+    !!! robot can do very quick movements !!!
+    """
+
+    # coordinate_specification options
+    coordinate_specification_base_coordinate = 0  # base coordinate option
+    coordinate_specification_robot_coordinate = 1  # robot coordinate option
+    coordinate_specification_user_1_coordinate = 2  # user 1 coordinate option
+
+    # Class constructor
+    def __init__(self, motion_speed_percentage, coordinate_specification,
+                 x, y, z, tx, ty, tz, d_10, d_11=0, d_12=0, d_13=0, d_14=0, d_15=0, d_16=0, d_17=0):
+        self.motion_speed_percentage = motion_speed_percentage
+        self.coordinate_specification = coordinate_specification
+        self.x = x
+        self.y = y
+        self.z = z
+        self.tx = tx
+        self.ty = ty
+        self.tz = tz
+        self.d_10 = d_10
+        self.d_11 = d_11
+        self.d_12 = d_12
+        self.d_13 = d_13
+        self.d_14 = d_14
+        self.d_15 = d_15
+        self.d_16 = d_16
+        self.d_17 = d_17
+
+    def get_command(self):
+        separator = ', '
+        return separator.join([
+            str(self.motion_speed_percentage),
+            str(self.coordinate_specification),
+            str(self.x),
+            str(self.y),
+            str(self.z),
+            str(self.tx),
+            str(self.ty),
+            str(self.tz),
+            str(self.d_10),
+            str(self.d_11),
+            str(self.d_12),
+            str(self.d_13),
+            str(self.d_14),
+            str(self.d_15),
+            str(self.d_16),
+            str(self.d_17)
+        ])
+
+    def get_coordinate_specification(self):
+        return self.coordinate_specification
+
+    def get_x(self):
+        return self.x
+
+    def get_y(self):
+        return self.y
+
+    def get_z(self):
+        return self.z
+
+    def get_tx(self):
+        return self.tx
+
+    def get_ty(self):
+        return self.ty
+
+    def get_tz(self):
+        return self.tz