├── requirements.txt ├── .gitignore ├── robstride ├── __init__.py ├── cli.py └── client.py ├── setup.py ├── LICENSE └── README.md /requirements.txt: -------------------------------------------------------------------------------- 1 | python-can >= 4.4.2 -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | robstride.egg-info 3 | robstride/__pycache__/ 4 | -------------------------------------------------------------------------------- /robstride/__init__.py: -------------------------------------------------------------------------------- 1 | from .client import Client, params, param_ids_by_name, RunMode -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | from setuptools import setup, find_packages 2 | 3 | def read_requirements(): 4 | with open('requirements.txt') as req: 5 | content = req.read() 6 | requirements = content.split('\n') 7 | return [r.strip() for r in requirements if r.strip() and not r.startswith('#')] 8 | 9 | setup( 10 | name="robstride", 11 | version="0.1.0", 12 | packages=find_packages(), 13 | install_requires=read_requirements(), 14 | entry_points={ 15 | "console_scripts": [ 16 | "robstride=robstride.cli:main", 17 | ], 18 | }, 19 | ) -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright 2024 Brian Smith 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the “Software”), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: 4 | 5 | The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. 6 | 7 | THE SOFTWARE IS PROVIDED “AS IS”, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. -------------------------------------------------------------------------------- /robstride/cli.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import sys 3 | 4 | import can 5 | 6 | from .client import Client, param_ids_by_name 7 | 8 | def main(): 9 | parser = argparse.ArgumentParser() 10 | parser.add_argument('--interface', type=str, help='The CAN interface to use') 11 | parser.add_argument('--channel', type=str, help='The channel for the CAN interface to use') 12 | 13 | subparsers = parser.add_subparsers(dest='command') 14 | 15 | parser_enable = subparsers.add_parser('enable') 16 | parser_enable.add_argument('motor_id', type=int, help='The ID of the motor to enable') 17 | 18 | parser_disable = subparsers.add_parser('disable') 19 | parser_disable.add_argument('motor_id', type=int, help='The ID of the motor to disable') 20 | 21 | parser_update_id = subparsers.add_parser('update_id') 22 | parser_update_id.add_argument('motor_id', type=int, help='The ID of the motor to update') 23 | parser_update_id.add_argument('new_motor_id', type=int, help='The new ID of the motor') 24 | 25 | parser_read = subparsers.add_parser('read') 26 | parser_read.add_argument('motor_id', type=int) 27 | parser_read.add_argument('param_name', type=str) 28 | 29 | parser_write = subparsers.add_parser('write') 30 | parser_write.add_argument('motor_id', type=int) 31 | parser_write.add_argument('param_name', type=str) 32 | parser_write.add_argument('param_value', type=float) 33 | 34 | args = parser.parse_args() 35 | if not args.command: 36 | parser.print_help() 37 | sys.exit(1) 38 | 39 | run(args) 40 | 41 | def run(args): 42 | with can.interface.Bus(interface=args.interface, channel=args.channel) as bus: 43 | client = Client(bus) 44 | if args.command == 'enable': 45 | client.enable(args.motor_id) 46 | elif args.command == 'disable': 47 | client.disable(args.motor_id) 48 | elif args.command == 'update_id': 49 | client.update_id(args.motor_id, args.new_motor_id) 50 | elif args.command == 'read': 51 | param_id = param_ids_by_name[args.param_name] 52 | value = client.read_param(args.motor_id, param_id) 53 | print(f'value: {value}') 54 | elif args.command == 'write': 55 | param_id = param_ids_by_name[args.param_name] 56 | client.write_param(args.motor_id, param_id, args.param_value) 57 | 58 | if __name__ == '__main__': 59 | main() -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Robstride Python SDK 2 | 3 | This is a simple SDK for communicating with Robstride motors in Python over CAN. 4 | 5 | It also includes a simple `robstride` CLI client for making ad-hoc calls to your motors. 6 | 7 | # Installation 8 | 9 | The simplest way to install is via `pip`: 10 | 11 | ```python3 -m pip install robstride``` 12 | 13 | # Basic usage 14 | 15 | This library is built on top of `python-can` and requires you to create a `can.Bus` object first. 16 | Once you've configured your can bus you can create a `robstride.Client`, which allows you to make 17 | calls to motors that are on the bus. 18 | 19 | ``` 20 | import can 21 | import robstride 22 | import time 23 | 24 | with can.Bus() as bus: 25 | rs_client = robstride.Client(bus) 26 | 27 | # First set the run mode to position 28 | rs_client.write(1, 'run_mode', robstride.RunMode.Position) 29 | 30 | # Then enable the motor 31 | rs_client.enable(1) 32 | 33 | # Next tell the motor to go to its zero position 34 | # Many calls including enable and write return the current state of the motor 35 | resp = rs_client.write(1, 'loc_ref', 0) 36 | print('starting position:', resp.angle) 37 | 38 | # Give is a few seconds to reach that position 39 | time.sleep(3) 40 | 41 | # Now read the position, this time through the read call. 42 | new_angle = rs_client.read(1, 'mech_pos') 43 | print('ending position:', new_angle) 44 | 45 | # Finally deactivate the motor 46 | rs_client.disable(1) 47 | ``` 48 | 49 | # Basic usage of the CLI 50 | 51 | All the calls that are supported by the library are also exposed via the `robstride` CLI client. It's especially useful for 52 | performing one-off tasks such as changing the motor ID, which default to 127. 53 | 54 | ``` 55 | # Update the motor ID 127 to 1 56 | robstride update-id 127 1 57 | ``` 58 | 59 | Other commands include `enable`, `disable`, `read`, and `write`. 60 | 61 | # Configuring your CAN bus 62 | 63 | Setting up your CAN bus depends on how you're commuinicating with CAN. If you're using a Canable compatible USB-CAN device, 64 | you can set up your can bus as follows: 65 | 66 | ``` 67 | # Configure the can0 device to use 1Mbps 68 | sudo ip link set can0 type can bitrate 1000000 loopback off 69 | 70 | # Enable can0 71 | sudo ifconfig can0 up 72 | ``` 73 | 74 | The simplest way to use this library is to configure your CAN setup in the `~/.can` file, which if you're using the setup above looks like this: 75 | ``` 76 | [default] 77 | interface = socketcan 78 | channel = can0 79 | ``` 80 | 81 | You can also configure these manually when creating the `can.Bus` object in code. 82 | For the `robstride` CLI client you can configure these values with the optional `--interface` and `--channel` CLI arguments. -------------------------------------------------------------------------------- /robstride/client.py: -------------------------------------------------------------------------------- 1 | import dataclasses 2 | import enum 3 | import math 4 | import struct 5 | from typing import List 6 | 7 | import can 8 | 9 | class RunMode(enum.Enum): 10 | Operation = 0 11 | Position = 1 12 | Speed = 2 13 | Current = 3 14 | 15 | class MotorMsg(enum.Enum): 16 | Info = 0 17 | Control = 1 18 | Feedback = 2 19 | Enable = 3 20 | Disable = 4 21 | ZeroPos = 6 22 | SetID = 7 23 | ReadParam = 17 24 | WriteParam = 18 25 | 26 | class MotorMode(enum.Enum): 27 | Reset = 0 28 | Calibration = 1 29 | Run = 2 30 | 31 | class MotorError(enum.Enum): 32 | Undervoltage = 1 33 | Overcurrent = 2 34 | Overtemp = 4 35 | MagneticEncodingFault = 8 36 | HallEncodingFault = 16 37 | Uncalibrated = 32 38 | 39 | @dataclasses.dataclass 40 | class FeedbackResp: 41 | servo_id: int 42 | errors: List[MotorError] 43 | mode: MotorMode 44 | angle: float 45 | velocity: float 46 | torque: float 47 | temp: float 48 | 49 | params = [ 50 | ('run_mode', 0x7005), 51 | ('iq_ref', 0x7006), 52 | ('spd_ref', 0x700A), 53 | ('limit_torque', 0x700B), 54 | ('cur_kp', 0x7010), 55 | ('cur_ki', 0x7011), 56 | ('cur_fit_gain', 0x7014), 57 | ('loc_ref', 0x7016), 58 | ('limit_spd', 0x7017), 59 | ('limit_cur', 0x7018), 60 | ('mechpos', 0x7019), 61 | ('iqf', 0x701A), 62 | ('mechvel', 0x701B), 63 | ('vbus', 0x701C), 64 | ('loc_kp', 0x701E), 65 | ('spd_kp', 0x701F), 66 | ('spd_ki', 0x7020), 67 | ('spd_filt_gain', 0x7021), 68 | ] 69 | 70 | param_ids_by_name = dict(params) 71 | 72 | class Client: 73 | def __init__(self, bus: can.BusABC, retry_count=2, recv_timeout=2, host_can_id=0xAA): 74 | self.bus = bus 75 | self.retry_count=retry_count 76 | self.recv_timeout = recv_timeout 77 | self.host_can_id = host_can_id 78 | self._recv_count = 0 79 | self._recv_error_count = 0 80 | 81 | def enable(self, motor_id: int, motor_model=1) -> FeedbackResp: 82 | self.bus.send(self._rs_msg(MotorMsg.Enable, self.host_can_id, motor_id, [0, 0, 0, 0, 0, 0, 0, 0])) 83 | resp = self._recv() 84 | return self._parse_feedback_resp(resp, motor_id, motor_model) 85 | 86 | def disable(self, motor_id: int, motor_model=1) -> FeedbackResp: 87 | self.bus.send(self._rs_msg(MotorMsg.Disable, self.host_can_id, motor_id, [0, 0, 0, 0, 0, 0, 0, 0])) 88 | resp = self._recv() 89 | return self._parse_feedback_resp(resp, motor_id, motor_model) 90 | 91 | def update_id(self, motor_id: int, new_motor_id: int): 92 | id_data_1 = self.host_can_id | (new_motor_id << 8) 93 | self.bus.send(self._rs_msg(MotorMsg.SetID, id_data_1, motor_id, [0, 0, 0, 0, 0, 0, 0, 0])) 94 | self._recv() 95 | 96 | def read_param(self, motor_id: int, param_id: int | str) -> float | RunMode: 97 | param_id = self._normalize_param_id(param_id) 98 | 99 | data = [param_id & 0xFF, param_id >> 8, 0, 0, 0, 0, 0, 0] 100 | self.bus.send(self._rs_msg(MotorMsg.ReadParam, self.host_can_id, motor_id, data)) 101 | resp = self._recv() 102 | 103 | self._parse_and_validate_resp_arbitration_id(resp, MotorMsg.ReadParam.value, motor_id) 104 | 105 | resp_param_id = struct.unpack(' FeedbackResp: 117 | param_id = self._normalize_param_id(param_id) 118 | 119 | data = bytes([param_id & 0xFF, param_id >> 8, 0, 0]) 120 | if param_id == 0x7005: 121 | if isinstance(param_value, RunMode): 122 | int_value = int(param_value.value) 123 | elif isinstance(param_value, int): 124 | int_value = param_value 125 | data += bytes([int_value, 0, 0, 0]) 126 | else: 127 | data += struct.pack(' float: 135 | return self._recv_error_count / self._recv_count 136 | 137 | def _rs_msg(self, msg_type, id_data_1, id_data_2, data): 138 | arb_id = id_data_2 + (id_data_1 << 8) + (msg_type.value << 24) 139 | return can.Message(arbitration_id=arb_id, data=data, is_extended_id=True) 140 | 141 | def _recv(self): 142 | retry_count = 0 143 | while retry_count <= self.retry_count: 144 | self._recv_count += 1 145 | resp = self.bus.recv(self.recv_timeout) 146 | if not resp: 147 | raise Exception('No response from motor received') 148 | if not resp.is_error_frame: 149 | return resp 150 | 151 | retry_count += 1 152 | self._recv_error_count += 1 153 | # TODO: make logging configurable 154 | print('received error:', resp) 155 | 156 | raise Exception('Error reading resp:', resp) 157 | 158 | def _parse_resp_abitration_id(self, aid): 159 | msg_type = (aid & 0x1F000000) >> 24 160 | msg_motor_id = (aid & 0xFF00) >> 8 161 | host_id = aid & 0xFF 162 | return msg_type, msg_motor_id, host_id 163 | 164 | def _parse_and_validate_resp_arbitration_id(self, resp, expected_msg_type, expected_motor_id): 165 | msg_type, msg_motor_id, host_id = self._parse_resp_abitration_id(resp.arbitration_id) 166 | if msg_type != expected_msg_type: 167 | raise Exception('Invalid msg_type', resp) 168 | if host_id != self.host_can_id: 169 | raise Exception('Invalid host CAN ID', resp) 170 | if msg_motor_id != expected_motor_id: 171 | raise Exception('Invalid motor ID received', resp) 172 | 173 | return msg_type, msg_motor_id, host_id 174 | 175 | def _parse_feedback_resp(self, resp, motor_id, motor_model): 176 | self._parse_and_validate_resp_arbitration_id(resp, MotorMsg.Feedback.value, motor_id) 177 | 178 | aid = resp.arbitration_id 179 | error_bits = (aid & 0x1F0000) >> 16 180 | errors = [] 181 | for i in range(6): 182 | value = 1 << i 183 | if value & error_bits: 184 | errors.append(MotorError(value)) 185 | 186 | mode = MotorMode((aid & 0x400000) >> 22) 187 | 188 | angle_raw = struct.unpack('>H', resp.data[0:2])[0] 189 | angle = (float(angle_raw) / 65535 * 8 * math.pi) - 4 * math.pi 190 | 191 | velocity_raw = struct.unpack('>H', resp.data[2:4])[0] 192 | velocity_range = 88 if motor_model == 1 else 30 193 | velocity = (float(velocity_raw) / 65535 * velocity_range) - velocity_range/2 194 | 195 | torque_raw = struct.unpack('>H', resp.data[4:6])[0] 196 | torque_range = 34 if motor_model == 1 else 240 197 | torque = (float(torque_raw) / 65535 * torque_range) - torque_range/2 198 | 199 | temp_raw = struct.unpack('>H', resp.data[6:8])[0] 200 | temp = float(temp_raw) / 10 201 | 202 | return FeedbackResp(motor_id, errors, mode, angle, velocity, torque, temp) 203 | 204 | def _normalize_param_id(self, param_id: int | str) -> int: 205 | if isinstance(param_id, str): 206 | return param_ids_by_name[param_id] 207 | 208 | return param_id --------------------------------------------------------------------------------