Initial commit

Author: vytasrgl

.gitignore

@@ -0,0 +1,4 @@
+*.pyc
+__pycache__
+.venv
+.vscode

README.md

@@ -0,0 +1,45 @@
+## REQUIREMENT
+ - Python 3.8
+ - Serial port access, for Ubuntu current user needs to be in dialout group
+## INSTALLATION
+For Ubuntu:
+
+```bash
+git clone https://github.com/hansonrobotics/servo_experiments
+cd servo_expriments
+python3.8 -m venv .venv --python /usr/bin/python3.8
+source .venv/bin/activate
+pip install -r requirements.txt
+```
+
+## Tools
+
+### Meassure
+Measures the time it takes for servo to move from one position to another
+Useage:
+```bash
+./meassure.py --help
+```
+```txt
+usage: meassure.py [-h] [--port PORT] [--baud BAUD] [--protocol PROTOCOL] [--id ID] [--from FROM] [--to TO] [--goback GOBACK] [--title TITLE] [--percieved PERCIEVED]
+
+Meassure servo speed based on feedback
+
+optional arguments:
+  -h, --help            show this help message and exit
+  --port PORT           Serial port (default: /dev/ttyUSB0)
+  --baud BAUD           Baudrate (default: 1000000)
+  --protocol PROTOCOL   SCS Protocol (default: 0)
+  --id ID               Servo ID (default: 1)
+  --from FROM           From Position (default: 500)
+  --to TO               To Position (default: 1000)
+  --goback GOBACK       Meassures time to get back to initial position as well (default: False)
+  --title TITLE         Title for graph (default: Servo Position)
+  --percieved PERCIEVED
+                        Remove first n percent of movement, to show percieved stats (default: 0)
+```
+Examples:
+```bash
+./meassure.py --id 1 --protocol 0 --from 300 --to 910 --goback true --percieved 5
+
+```

meassure.py

@@ -0,0 +1,148 @@
+#!/usr/bin/env python
+
+
+from ast import Return
+from dataclasses import dataclass
+import time
+import argparse
+import time
+import scservo_sdk as sdk
+import matplotlib.pyplot as plt
+import numpy as np
+
+# Control table address
+ADDR_SCS_TORQUE_ENABLE     = 40
+ADDR_SCS_GOAL_ACC          = 41
+ADDR_SCS_GOAL_POSITION     = 42
+ADDR_SCS_GOAL_SPEED        = 46
+ADDR_SCS_PRESENT_POSITION  = 56
+MOVING_THRESHOLD = 5
+STEPS_IN_DEG = {
+    0: 11.38,
+    1: 4.65,
+}
+
+def argument_parser():
+    parser = argparse.ArgumentParser(description="Meassure servo speed based on feedback", formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+    parser.add_argument("--port", type=str, default='/dev/ttyUSB0', help='Serial port')
+    parser.add_argument("--baud", type=int, default=1000000, help='Baudrate')
+    parser.add_argument("--protocol", type=int, default=0, help='SCS Protocol')
+    parser.add_argument("--id", type=int, default=1, help='Servo ID')
+    parser.add_argument("--from", type=int, default=500, help='From Position')
+    parser.add_argument("--to", type=int, default=1000, help='To Position')
+    parser.add_argument("--goback", type=bool, default=False, help='Meassures time to get back to initial position as well')
+    parser.add_argument("--title", type=str, default="Servo Position", help='Title for graph')
+    parser.add_argument("--percieved", type=int, default=0, help='Remove first n percent of movement, to show percieved stats')
+
+    args = parser.parse_args()
+    return vars(args)
+
+
+
+def plot(x, y, xlabel='Time (s)', ylabel='Degrees', title='Servo Position'):
+    plt.plot(x, y)
+    plt.xlabel(xlabel)
+    plt.ylabel(ylabel)
+    plt.title(title)
+    plt.show()
+
+
+def move_to(group_sync_write, id, position):
+    param_goal_position = [sdk.SCS_LOBYTE(position), sdk.SCS_HIBYTE(position)]
+    group_sync_write.addParam(id, param_goal_position)
+    group_sync_write.txPacket()
+    group_sync_write.clearParam()
+
+def move_to_single(port, packets, id, position):
+    packets.write2ByteTxRx(port, id, ADDR_SCS_GOAL_POSITION, position)
+
+def current_position(group_sync_read, id, ph=None):
+    scs_comm_result = group_sync_read.txRxPacket()
+    if scs_comm_result != 0:
+        print("Error")
+        print(ph.getTxRxResult(scs_comm_result))
+
+    data = group_sync_read.getData(id, ADDR_SCS_PRESENT_POSITION, 2)
+    return data
+
+def current_position_single(port, packets, id):
+    data, status, error = packets.read2ByteTxRx(port, id, ADDR_SCS_PRESENT_POSITION)
+    if status or error:
+        raise Exception(f"Error reading position: {status} {error}")
+    return data
+
+
+
+
+def main():
+    args = argument_parser()
+    port = sdk.PortHandler(args['port'])
+    print(args['protocol'])
+
+    packets = sdk.PacketHandler(args['protocol'])
+    if not port.openPort() and port.setBaudRate(args['baud']):
+        print(f"Cant Open port {args['port']}")
+        return
+    id = args['id']
+    scs_model_number, scs_comm_result, scs_error = packets.ping(port, id)
+    if scs_comm_result or scs_error:
+        print("Cant ping servo")
+        return
+    else:
+        print(f"Servo with ID: {id} is found. Model {scs_model_number}")
+    times, positions = meassure(port, packets, id, args['from'], args['to'], args['goback'], STEPS_IN_DEG[args['protocol']])
+    if args['percieved'] > 0:
+        percieved_angle_threshold = (args['to'] - args['from']) * args['percieved'] / 100 / STEPS_IN_DEG[args['protocol']]
+        print(percieved_angle_threshold)
+        times, positions = perceived_data(times, positions, percieved_angle_threshold)
+        print(f'Starting: {times[0]}, duration: {times[-1] - times[0]}')
+
+    plot(times, positions, args['title'])
+
+def perceived_data(times, positions,threshold):
+    percieved_times = []
+    percieved_positions = []
+    for p,t in zip(positions, times):
+        if abs(p) > abs(threshold):
+            percieved_positions.append(p)
+            percieved_times.append(t)
+    return percieved_times, percieved_positions
+
+def meassure(port,packets, id, start, end, back=True, steps_in_deg=1):
+    group_sync_read = sdk.GroupSyncRead(port, packets, ADDR_SCS_PRESENT_POSITION, 2)
+    group_sync_read.addParam(id)
+    group_sync_write = sdk.GroupSyncWrite(port, packets, ADDR_SCS_GOAL_POSITION, 2)
+    move_to(group_sync_write, id, start)
+    print("Moving to initial position")
+    time.sleep(1)
+    #starting = current_position(group_sync_read, id, packets)
+    starting = current_position_single(port, packets, id)
+    time.sleep(1)
+    print(f"Starting position: {starting}")
+    # position
+    positions = [starting]
+    start_time = time.perf_counter()
+    times = [0]
+    current = starting
+    move_to(group_sync_write, id, end)
+    #move_to_single(port, packets, id, end)
+    goal = end
+    while True:
+        current = current_position_single(port, packets, id)
+        positions.append(current)
+        times.append(time.perf_counter() - start_time)
+        if abs(goal-current) < MOVING_THRESHOLD + 50*back:
+            if back:
+                back = False
+                goal = start
+                move_to(group_sync_write, id, goal)
+            else:
+                break
+    if back:
+       move_to(group_sync_write, id, start)
+    print(f"Movement finished in {times[-1]} seconds. samples {len(times)}")
+    positions = [(p-start)/steps_in_deg for p in positions]
+    return times, positions
+
+if __name__ == '__main__':
+    main()

requirements.txt

@@ -0,0 +1,3 @@
+pyqt5
+matplotlib
+pyserial

scservo_sdk/__init__.py

@@ -0,0 +1,6 @@
+#!/usr/bin/env python
+
+from .port_handler import *
+from .packet_handler import *
+from .group_sync_read import *
+from .group_sync_write import *

scservo_sdk/group_sync_read.py

@@ -0,0 +1,110 @@
+#!/usr/bin/env python
+
+from .scservo_def import *
+
+class GroupSyncRead:
+    def __init__(self, port, ph, start_address, data_length):
+        self.port = port
+        self.ph = ph
+        self.start_address = start_address
+        self.data_length = data_length
+
+        self.last_result = False
+        self.is_param_changed = False
+        self.param = []
+        self.data_dict = {}
+
+        self.clearParam()
+
+    def makeParam(self):
+        if not self.data_dict:  # len(self.data_dict.keys()) == 0:
+            return
+
+        self.param = []
+
+        for scs_id in self.data_dict:
+            self.param.append(scs_id)
+
+    def addParam(self, scs_id):
+        if scs_id in self.data_dict:  # scs_id already exist
+            return False
+
+        self.data_dict[scs_id] = []  # [0] * self.data_length
+
+        self.is_param_changed = True
+        return True
+
+    def removeParam(self, scs_id):
+        if scs_id not in self.data_dict:  # NOT exist
+            return
+
+        del self.data_dict[scs_id]
+
+        self.is_param_changed = True
+
+    def clearParam(self):
+        self.data_dict.clear()
+
+    def txPacket(self):
+        if len(self.data_dict.keys()) == 0:
+            return COMM_NOT_AVAILABLE
+
+        if self.is_param_changed is True or not self.param:
+            self.makeParam()
+
+        return self.ph.syncReadTx(self.port, self.start_address, self.data_length, self.param,
+                                  len(self.data_dict.keys()) * 1)
+
+    def rxPacket(self):
+        self.last_result = False
+
+        result = COMM_RX_FAIL
+
+        if len(self.data_dict.keys()) == 0:
+            return COMM_NOT_AVAILABLE
+
+        for scs_id in self.data_dict:
+            self.data_dict[scs_id], result, _ = self.ph.readRx(self.port, scs_id, self.data_length)
+            if result != COMM_SUCCESS:
+                return result
+
+        if result == COMM_SUCCESS:
+            self.last_result = True
+
+        return result
+
+    def txRxPacket(self):
+        result = self.txPacket()
+        if result != COMM_SUCCESS:
+            return result
+
+        return self.rxPacket()
+
+    def isAvailable(self, scs_id, address, data_length):
+        #if self.last_result is False or scs_id not in self.data_dict:
+        if scs_id not in self.data_dict:
+            return False
+
+        if (address < self.start_address) or (self.start_address + self.data_length - data_length < address):
+            return False
+
+        if len(self.data_dict[scs_id])<data_length:
+            return False
+        return True
+
+    def getData(self, scs_id, address, data_length):
+        if not self.isAvailable(scs_id, address, data_length):
+            return 0
+
+        if data_length == 1:
+            return self.data_dict[scs_id][address - self.start_address]
+        elif data_length == 2:
+            return SCS_MAKEWORD(self.data_dict[scs_id][address - self.start_address],
+                                self.data_dict[scs_id][address - self.start_address + 1])
+        elif data_length == 4:
+            return SCS_MAKEDWORD(SCS_MAKEWORD(self.data_dict[scs_id][address - self.start_address + 0],
+                                              self.data_dict[scs_id][address - self.start_address + 1]),
+                                 SCS_MAKEWORD(self.data_dict[scs_id][address - self.start_address + 2],
+                                              self.data_dict[scs_id][address - self.start_address + 3]))
+        else:
+            return 0