├── LICENSE ├── README.md ├── cpp ├── interbotix_arm.cpp ├── linux32 │ └── Makefile ├── linux64 │ └── Makefile ├── linux_sbc │ └── Makefile └── mac │ └── Makefile └── python └── interbotix_arm.py /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2021, Interbotix 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # DynamixelSDK XSeries Arm Examples 2 | 3 | ## Overview 4 | Welcome to the *dynamixelsdk_xsarm_examples* repository! This repo contains example code written in both C++ and Python which can be used to control any Interbotix XSeries Arm. To accomplish this, the DynamixelSDK [C++](https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/api_reference/cpp/cpp_porthandler/#cpp-porthandler) and [Python](https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/api_reference/python/python_porthandler/#python-porthandler) APIs were used. Additionally, the C++ code was loosely based on the [read_write.cpp](https://github.com/ROBOTIS-GIT/DynamixelSDK/blob/master/c%2B%2B/example/protocol2.0/read_write/read_write.cpp) and [sync_read_write.cpp](https://github.com/ROBOTIS-GIT/DynamixelSDK/blob/master/c%2B%2B/example/protocol2.0/sync_read_write/sync_read_write.cpp) examples provided by Robotis in the [DynamixelSDK](https://github.com/ROBOTIS-GIT/DynamixelSDK) repository. Similarly, the Python code was loosely based on the [read_write.py](https://github.com/ROBOTIS-GIT/DynamixelSDK/blob/master/python/tests/protocol2_0/read_write.py) and [sync_read_write.py](https://github.com/ROBOTIS-GIT/DynamixelSDK/blob/master/python/tests/protocol2_0/sync_read_write.py) examples as well. 5 | 6 | ## Compatible Products 7 | 8 | The examples are based on the Protocol 2.0 communication protocol, so any arm made with Protocol 2.0 compatible Dynamixel servos can be used. Note that while the examples are specific to the WidowX 200 arm, it can be easily modified to work with any of the XSeries arms shown below. 9 | 10 | - [PincherX 100 Robot Arm](https://www.trossenrobotics.com/pincherx-100-robot-arm.aspx) 11 | - [PincherX 150 Robot Arm](https://www.trossenrobotics.com/pincherx-150-robot-arm.aspx) 12 | - [ReactorX 150 Robot Arm](https://www.trossenrobotics.com/reactorx-150-robot-arm.aspx) 13 | - [ReactorX 200 Robot Arm](https://www.trossenrobotics.com/reactorx-200-robot-arm.aspx) 14 | - [WidowX 200 Robot Arm](https://www.trossenrobotics.com/widowx-200-robot-arm.aspx) 15 | - [WidowX 250 Robot Arm](https://www.trossenrobotics.com/widowx-250-robot-arm.aspx) 16 | - [WidowX 250 Robot Arm 6DOF](https://www.trossenrobotics.com/widowx-250-robot-arm-6dof.aspx) 17 | - [ViperX 250 Robot Arm](https://www.trossenrobotics.com/viperx-250-robot-arm.aspx) 18 | - [ViperX 300 Robot Arm](https://www.trossenrobotics.com/viperx-300-robot-arm.aspx) 19 | - [ViperX 300 Robot Arm 6DOF](https://www.trossenrobotics.com/viperx-300-robot-arm-6dof.aspx) 20 | - [PincherX 100 Mobile Robot Arm](https://www.trossenrobotics.com/pincherx-100-mobile-robot-arm.aspx) 21 | - [WidowX 200 Mobile Robot Arm](https://www.trossenrobotics.com/widowx-200-robot-arm-mobile-base.aspx) 22 | - [WidowX 250 Mobile Robot Arm 6DOF](https://www.trossenrobotics.com/widowx-250-mobile-robot-arm-6dof.aspx) 23 | 24 | ## Requirements 25 | Below is a list of the hardware you will need to get started. 26 | - Computer (any operating system compatible with the DynamixelSDK) 27 | - One of the X-Series Robot Arm Kits mentioned above 28 | 29 | ## Hardware Setup 30 | There is not much required to get the robot ready to work as most of the setup is done for you. Just make sure to do the following steps: 31 | 1. Remove the robot from its packaging and place on a sturdy tabletop surface near an electrical outlet. To prevent the robot from potentially toppling during operation, secure it to a flat surface (via clamping or using the holes on the base's perimeter). At your own risk, you could instead place a small heavy bean-bag on top of the acrylic plate by the base of the robot. Finally, make sure that there are no obstacles within the workspace of the arm. 32 | 2. Plug the 12V power cable into an outlet and insert the barrel plug into the barrel jack on the X-series power hub (located under the see-through acrylic on the base of the robot). You should briefly see the LEDs on the Dynamixel motors flash red. 33 | 3. Plug in the micro-usb cable into the U2D2 (located under the see-through acrylic on the robot's base) and your computer. 34 | 35 | ## Software Setup 36 | To get all the code setup, refer to Robotis's documentation [here](https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/download/#repository) to download the various APIs, and the instructions [here](https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/library_setup/c_windows/#c-windows) to install them (click the appropriate library on the left-hand side). Then download a zip file with the code or clone this repository wherever you'd like on your computer by typing... 37 | ``` 38 | $ git clone https://github.com/Interbotix/dynamixelsdk_xsarm_examples.git 39 | ``` 40 | If you're going the 'cpp' route (at least in Linux), navigate to the directory that contains the Makefile and type `make`. An executable file should pop up called 'interbotix_arm'. To run it, just type `./interbotix_arm`. 41 | 42 | ## Structure 43 | Both the 'Cpp' and 'Python' examples in this repo do the exact same thing so that you can get a feel for which language you prefer to use. Also, the code in this repo is pretty 'lightweight' in that all code is kept to just a single file. This is to make it easier for you to see how to *implement* the DynamixelSDK without jumping all over the place to view Python modules or Cpp header files. In that sense, the code shown here is meant to act as a starting point for your own development. 44 | 45 | The structure for both files is pretty straightforward and contains two parts. The first part is the largest and just contains many helper functions that can be used to work with the motors. This includes functions that allow you to read/write from/to a single motor or many motors sequentially. It also includes functions to allow you to read/write to multiple motors simultaneously (using the 'sync read/write' feature of the Dynamixel SDK). Note that the main benefit of communicating simultaneously with the motors is that it takes much less time than communicating sequentially, and all data has the same 'time stamp'. For example, if you command the arm motors to move to your desired goal positions simultaneously (and specify each one to take 1.5 seconds), all motors will start and stop at the same time. However, if you were to command the arm motors to move to your desired goal positions sequentially in the same manner, the motors will each start at different times and finish their motions at different times. 46 | 47 | Another helper function defined in the first part of both files is called 'CalibrateDualJoint'. This function can be used to calibrate a joint that is composed from two motors (like the shoulder joint in our WidowX 200 arm, or the elbow joint in our WidowX 250 arm). Essentially, the issue that can arise is that the encoder on each motor can show slightly different positions despite having their horns physically aligned during robot assembly. As a result, if a goal position is commanded to that joint, one motor might reach the goal position and stop moving. However, the other motor's encoder might show that the goal position hasn't been reached yet and exert more effort to reach it. As a result, the first motor will exert more effort to keep holding its position. This leads to a vicious cycle that can ultimately lead to one or both motors overloading and torquing off. To prevent that from happening, the 'calibrateDualJoint' function sets the Homing Offset register of the second motor in a dual joint setup so that the second motor's encoder shows the same value as the first motor's encoder. 48 | 49 | The second part of both files is the 'main' function where you write code to interact with the robot. It consists of defining vectors/lists of the IDs that make up the arm as well as initial values that should be sent to those motors' registers. It also entails the *writing* of those initial values to the motors. This portion of the code should be modified depending on which robot arm is being used. Following that 'initial setup part', the code shows how to move the arm, open/close the gripper, and get data like current temperatures and PWMs or currents. 50 | 51 | As an FYI, the code does not contain any IK Solvers or higher level APIs. It is only meant to showcase how to implement the DynamixelSDK API so that you don't have to try figuring it out on your own. 52 | 53 | ## Troubleshooting 54 | For help using the examples, feel free to submit an Issue. Just please include your operating system, any applicable error messages, and the commands you ran as well. You can also contact us directly at trsupport@trossenrobotics.com, but we recommend submitting an Issue so that other people who may be facing the same difficulty can benefit. Note that this repository is actively maintained and any open Issues will be addressed as soon as possible. 55 | 56 | ## Contributing 57 | Feel free to send PRs to add features (like demos in other languages) or bug fixes. All PRs should follow the same type of organizational structure and documentation as shown in this repo. 58 | 59 | ## Contributors 60 | - [Solomon Wiznitzer](https://github.com/swiz23) - **Software Engineer** 61 | -------------------------------------------------------------------------------- /cpp/interbotix_arm.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #define ADDR_DRIVE_MODE 10 6 | #define ADDR_OPERATING_MODE 11 7 | #define ADDR_SECONDARY_ID 12 8 | #define ADDR_HOMING_OFFSET 20 9 | #define ADDR_MAX_POS_LIMIT 48 10 | #define ADDR_MIN_POS_LIMIT 52 11 | #define ADDR_TORQUE_ENABLE 64 12 | #define ADDR_POSITION_P_GAIN 84 13 | #define ADDR_GOAL_PWM 100 14 | #define ADDR_PROFILE_ACCELERATION 108 15 | #define ADDR_PROFILE_VELOCITY 112 16 | #define ADDR_GOAL_POSITION 116 17 | #define ADDR_PRESENT_LOAD 126 18 | #define ADDR_PRESENT_POSITION 132 19 | #define ADDR_PRESENT_TEMP 146 20 | 21 | #define PI 3.14159265 22 | 23 | dynamixel::PortHandler *portHandler; 24 | dynamixel::PacketHandler *packetHandler; 25 | 26 | /// @brief Helper function to check error messages and print them if need be 27 | /// @param dxl_error - if nonzero, there is an error in something related to the data 28 | /// @param dxl_comm_result - if nonzero, there is an error in the communication 29 | /// @return - true if no error occurred; false otherwise 30 | bool checkError(uint8_t dxl_error, int dxl_comm_result) 31 | { 32 | if (dxl_comm_result != COMM_SUCCESS) 33 | { 34 | printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result)); 35 | return false; 36 | } 37 | else if (dxl_error != 0) 38 | { 39 | printf("%s\n", packetHandler->getRxPacketError(dxl_error)); 40 | return false; 41 | } 42 | return true; 43 | } 44 | 45 | /// @brief Writes data to the specified register for a given motor 46 | /// @param id - Dynamixel ID to write data to 47 | /// @param address - register number 48 | /// @param data - data to write (user must provide the correct type!!!) 49 | /// @return - true if data was successfully written; false otherwise 50 | template 51 | bool itemWrite(uint8_t id, uint16_t address, T data) 52 | { 53 | uint8_t dxl_error = 0; 54 | int dxl_comm_result; 55 | if (sizeof(T) == 1) 56 | dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, id, address, data, &dxl_error); 57 | else if (sizeof(T) == 2) 58 | dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, id, address, data, &dxl_error); 59 | else if (sizeof(T) == 4) 60 | dxl_comm_result = packetHandler->write4ByteTxRx(portHandler, id, address, data, &dxl_error); 61 | else 62 | { 63 | printf("Invalid type..."); 64 | return false; 65 | } 66 | return checkError(dxl_error, dxl_comm_result); 67 | } 68 | 69 | /// @brief Sequentially writes the same data to the specified register for a group of motors 70 | /// @param id - vector of IDs to write data to 71 | /// @param address - register number 72 | /// @param data - data to write (user must provide the correct type!!!) 73 | /// @return - true if data was successfully written; false otherwise 74 | template 75 | bool itemWriteMultiple(std::vector *ids, uint16_t address, T data) 76 | { 77 | for (auto const& id : *ids) 78 | { 79 | if (itemWrite(id, address, data) != true) 80 | return false; 81 | } 82 | return true; 83 | } 84 | 85 | /// @brief Sequentially writes different data to the specified register for a group of motors 86 | /// @param id - vector of IDs to write data to 87 | /// @param address - register number 88 | /// @param data - data to write for each motor (user must provide the correct type!!!) 89 | /// @return - true if data was successfully written; false otherwise 90 | template 91 | bool itemWriteMultiple(std::vector *ids, uint16_t address, std::vector *data) 92 | { 93 | for (size_t i{0}; i < ids->size(); i++) 94 | { 95 | if (itemWrite(ids->at(i), address, data->at(i)) != true) 96 | return false; 97 | } 98 | return true; 99 | } 100 | 101 | /// @brief Reads data from the specified register for a given motor 102 | /// @param id - Dynamixel ID to read data from 103 | /// @param address - register number 104 | /// @param state - variable to store the requested data (user must provide the correct type!!!) 105 | /// @return - true if data was successfully retrieved; false otherwise 106 | template 107 | bool itemRead(uint8_t id, uint16_t address, T *state) 108 | { 109 | uint8_t dxl_error = 0; 110 | int dxl_comm_result; 111 | 112 | if (sizeof(T) == 1) 113 | { 114 | uint8_t raw_data; 115 | dxl_comm_result = packetHandler->read1ByteTxRx(portHandler, id, address, &raw_data, &dxl_error); 116 | *state = raw_data; 117 | } 118 | else if (sizeof(T) == 2) 119 | { 120 | uint16_t raw_data; 121 | dxl_comm_result = packetHandler->read2ByteTxRx(portHandler, id, address, &raw_data, &dxl_error); 122 | *state = raw_data; 123 | } 124 | else if (sizeof(T) == 4) 125 | { 126 | uint32_t raw_data; 127 | dxl_comm_result = packetHandler->read4ByteTxRx(portHandler, id, address, &raw_data, &dxl_error); 128 | *state = raw_data; 129 | } 130 | else 131 | { 132 | printf("Invalid data type."); 133 | return false; 134 | } 135 | return checkError(dxl_error, dxl_comm_result); 136 | } 137 | 138 | /// @brief Sequentially reads data from the specified register for a group of motors 139 | /// @param id - vector of Dynamixel IDs to read data from 140 | /// @param address - register number 141 | /// @param states - vector to store the requested data for each motor (user must provide the correct type!!!) 142 | /// @return - true if data was successfully retrieved; false otherwise 143 | template 144 | bool itemReadMultiple(std::vector *ids, uint16_t address, std::vector *states) 145 | { 146 | T data; 147 | states->reserve(ids->size()); 148 | for (auto const& id : *ids) 149 | { 150 | if (itemRead(id, address, &data) != true) 151 | return false; 152 | states->push_back(data); 153 | } 154 | return true; 155 | } 156 | 157 | /// @brief Writes data to a group of motors synchronously 158 | /// @param groupSyncWrite - groupSyncWrite object 159 | /// @param ids - vector of Dynamixel IDs to write to 160 | /// @param commands - vector of commands to write to each respective motor (user must provide the correct type!!!) 161 | /// @return - true if data was successfully written; false otherwise 162 | template 163 | bool syncWrite(dynamixel::GroupSyncWrite *groupSyncWrite, std::vector *ids, std::vector *commands) 164 | { 165 | groupSyncWrite->clearParam(); 166 | bool dxl_addparam_result = false; 167 | uint8_t param[sizeof(T)] = {0}; 168 | 169 | for (size_t i{0}; i < commands->size(); i++) 170 | { 171 | T data = commands->at(i); 172 | if (sizeof(T) == 4) 173 | { 174 | param[0] = DXL_LOBYTE(DXL_LOWORD(data)); 175 | param[1] = DXL_HIBYTE(DXL_LOWORD(data)); 176 | param[2] = DXL_LOBYTE(DXL_HIWORD(data)); 177 | param[3] = DXL_HIBYTE(DXL_HIWORD(data)); 178 | } 179 | else if (sizeof(T) == 2) 180 | { 181 | param[0] = DXL_LOBYTE(data); 182 | param[1] = DXL_HIBYTE(data); 183 | } 184 | else 185 | { 186 | param[0] = data; 187 | } 188 | dxl_addparam_result = groupSyncWrite->addParam(ids->at(i), param); 189 | if (dxl_addparam_result != true) 190 | { 191 | printf("ID:%03d groupSyncWrite addparam failed", ids->at(i)); 192 | return false; 193 | } 194 | } 195 | 196 | int dxl_comm_result = groupSyncWrite->txPacket(); 197 | if (dxl_comm_result != COMM_SUCCESS) 198 | { 199 | printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result)); 200 | return false; 201 | } 202 | return true; 203 | } 204 | 205 | /// @brief Reads data from a group of motors synchronously 206 | /// @param groupSyncRead - groupSyncRead object 207 | /// @param ids - vector of Dynamixel IDs to read from 208 | /// @param address - register number 209 | /// @param states - vector to store the requested data for each motor (user must provide the correct type!!!) 210 | /// @return - true if data was successfully read; false otherwise 211 | template 212 | bool syncRead(dynamixel::GroupSyncRead *groupSyncRead, std::vector *ids, uint16_t address, std::vector *states) 213 | { 214 | groupSyncRead->clearParam(); 215 | bool dxl_addparam_result = false; 216 | for (auto const& id : *ids) 217 | { 218 | dxl_addparam_result = groupSyncRead->addParam(id); 219 | if (dxl_addparam_result != true) 220 | { 221 | printf("ID:%03d groupSyncRead addparam failed", id); 222 | return false; 223 | } 224 | } 225 | 226 | bool dxl_comm_result = groupSyncRead->txRxPacket(); 227 | if (dxl_comm_result != COMM_SUCCESS) 228 | { 229 | printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result)); 230 | return false; 231 | } 232 | 233 | uint16_t data_length = sizeof(T); 234 | states->reserve(ids->size()); 235 | for (auto const& id : *ids) 236 | states->push_back(groupSyncRead->getData(id, address, data_length)); 237 | return true; 238 | } 239 | 240 | /// @brief Calibrates a dual-motor joint so that both motors show the same Present Position value 241 | /// @param master_id - in a dual-motor joint setup, this is the ID with the lower value 242 | /// @param slave_id - in a dual-motor joint setup, this is the ID with the higher value 243 | /// @details - note that the Drive mode must be set beforehand for both motors (specifically if CW or CCW is positive) 244 | void calibrateDualJoint(uint8_t master_id, uint8_t slave_id) 245 | { 246 | uint8_t slave_drive_mode; 247 | int32_t master_position, slave_position, homing_offset; 248 | itemWrite(slave_id, ADDR_HOMING_OFFSET, (int32_t)0); 249 | itemRead(master_id, ADDR_PRESENT_POSITION, &master_position); 250 | itemRead(slave_id, ADDR_PRESENT_POSITION, &slave_position); 251 | itemRead(slave_id, ADDR_DRIVE_MODE, &slave_drive_mode); 252 | bool slave_forward = (slave_drive_mode % 2 == 0); 253 | printf("Master Position: %d\nSlave Position: %d\n", master_position, slave_position); 254 | if (slave_forward) 255 | homing_offset = master_position - slave_position; 256 | else 257 | homing_offset = slave_position - master_position; 258 | printf("Setting Slave Homing Offset Register to: %d.\n", homing_offset); 259 | itemWrite(slave_id, ADDR_HOMING_OFFSET, homing_offset); 260 | } 261 | 262 | /// @brief Initializes the port that the U2D2 is connected to 263 | /// @param port_name - name of the port 264 | /// @baudrate - desired baudrate in bps (should be the same as the motors) 265 | /// @return - true if the port was initialized; false otherwise 266 | bool initPort(const char *port_name, const int baudrate) 267 | { 268 | portHandler = dynamixel::PortHandler::getPortHandler(port_name); 269 | packetHandler = dynamixel::PacketHandler::getPacketHandler(2.0); 270 | 271 | // Open port 272 | if (portHandler->openPort()) 273 | printf("Successfully opened the port at %s!\n", port_name); 274 | else 275 | { 276 | printf("Failed to open the port at %s!\n", port_name); 277 | return false; 278 | } 279 | 280 | // Set port baudrate 281 | if (portHandler->setBaudRate(baudrate)) 282 | printf("Succeeded to change the baudrate to %d bps!\n", baudrate); 283 | else 284 | { 285 | printf("Failed to change the baudrate to %d bps!\n", baudrate); 286 | return false; 287 | } 288 | 289 | return true; 290 | } 291 | 292 | /// @brief Ping the desired motors to verify their existence 293 | /// @param ids - vector of Dynamixel IDs to ping 294 | /// @return - true if all IDs were pinged successfully 295 | bool ping(std::vector ids) 296 | { 297 | bool ping_successful = true; 298 | for (auto const& id : ids) 299 | { 300 | uint8_t error = 0; 301 | int dxl_comm_result = COMM_TX_FAIL; 302 | if (packetHandler->ping(portHandler, id, &error) == COMM_SUCCESS) 303 | printf("Pinged ID: %03d successfully.\n", id); 304 | else 305 | { 306 | printf("%s\n", packetHandler->getTxRxResult(dxl_comm_result)); 307 | ping_successful = false; 308 | } 309 | } 310 | return ping_successful; 311 | } 312 | 313 | /// @brief Convert a raw Position register value into radians 314 | /// @param value - desired raw value to convert 315 | /// @return - value in radians 316 | float convertValue2Radian(int32_t value) 317 | { 318 | return (value - 2047.5)/2047.5 * PI; 319 | } 320 | 321 | /// @brief Convert a value in radians to the raw Position register value 322 | /// @param radian - desired value in radians to convert 323 | /// @return - raw register value 324 | int32_t convertRadian2Value(float radian) 325 | { 326 | return round(radian/PI * 2047.5 + 2047.5); 327 | } 328 | 329 | int main() 330 | { 331 | // WX200 ARM EEPROM CONFIGS 332 | std::vector all_ids = {1,2,3,4,5,6,7}; // Note that each index in the following vectors correspond to the ID in the matching index in the 'all_ids' vector 333 | std::vector drive_modes = {4,4,5,5,5,4,4}; // '4' means CCW is positive and Time-Based profile is being used; '5' means CCW is negative and Time-Based profile is being used 334 | std::vector shadow_ids = {255,255,2,255,255,255,255}; // '255' means shadow id is disabled; '0 - 252' means follow the motor with the specified ID 335 | std::vector min_position_limits = {0,0,0,0,0,0,0}; 336 | std::vector max_position_limits = {4095,4095,4095,4095,4095,4095,4095}; 337 | std::vector operating_modes = {3,3,3,3,3,3,16}; // '3' is position mode while '16' is PWM mode 338 | std::vector position_p_gains = {800, 1500, 2000, 1000, 640}; // Proportional Gain in the internal PID control loop in each motor 339 | 340 | // WX200 ARM 'Command' IDs 341 | std::vector arm_ids = {1,2,4,5,6}; // WX200 Arm Motor IDs (excludes slaves since they will automatically follow the masters and excludes grippers) 342 | uint8_t gripper_id = 7; // WX200 Gripper Motor ID 343 | uint8_t shoulder_master_id = 2; // WX200 Shoulder Master ID (in dual-joint setup) 344 | uint8_t shoulder_slave_id = 3; // WX200 Shoulder Slave ID (in dual-joint setup) 345 | 346 | // Initialize the port, ping the motors, and create syncWrite and syncRead objects 347 | // It's faster and better design to read/write motors with the 'sync' objects than to command each motor sequentially 348 | // Commanding each motor sequentially should only be done for 'non-realtime sensitive' registers - like torquing on/off, setting EEPROM registers, etc... 349 | if (!initPort("/dev/ttyDXL", 1000000)) 350 | return 1; 351 | if (!ping(all_ids)) 352 | return 1; 353 | dynamixel::GroupSyncWrite groupSyncWrite(portHandler, packetHandler, ADDR_GOAL_POSITION, 4); 354 | dynamixel::GroupSyncRead groupSyncRead(portHandler, packetHandler, ADDR_PRESENT_POSITION, 4); 355 | 356 | // Initialize register values 357 | itemWriteMultiple(&all_ids, ADDR_TORQUE_ENABLE, (uint8_t)0); // torque off all motors so that we can write to the EEPROM registers 358 | itemWriteMultiple(&all_ids, ADDR_DRIVE_MODE, &drive_modes); // set the Drive_Mode register for all motors 359 | itemWriteMultiple(&all_ids, ADDR_SECONDARY_ID, &shadow_ids); // set the Shadow ID register for all motors 360 | itemWriteMultiple(&all_ids, ADDR_MIN_POS_LIMIT, &min_position_limits); // set the Min Position Limit for all motors 361 | itemWriteMultiple(&all_ids, ADDR_MAX_POS_LIMIT, &max_position_limits); // set the Max Position Limit for all motors 362 | itemWriteMultiple(&all_ids, ADDR_OPERATING_MODE, &operating_modes); // set the Operating Mode register for all motors 363 | itemWriteMultiple(&arm_ids, ADDR_POSITION_P_GAIN, &position_p_gains); // raise the internal Position P Gain in the motors' PID loops 364 | itemWriteMultiple(&arm_ids, ADDR_PROFILE_VELOCITY, (uint32_t)1500); // set the Profile Velocity register for all arm motors to 1500ms (each movement takes 1.5 second) 365 | itemWriteMultiple(&arm_ids, ADDR_PROFILE_ACCELERATION, (uint32_t)750); // set the Profile Acceleration register for all arm motors to 750ms (each movement should take 0.75 sec to acclerate and 0.75 sec to decelerate) 366 | calibrateDualJoint(shoulder_master_id, shoulder_slave_id); // Calibrate the Homing Offset Register in the Slave motor until its Present Position register is equivalent to the Master's 367 | itemWriteMultiple(&all_ids, ADDR_TORQUE_ENABLE, (uint8_t)1); // torque all motors on 368 | 369 | // Read current arm joint positions 370 | std::vector positions; 371 | syncRead(&groupSyncRead, &arm_ids, ADDR_PRESENT_POSITION, &positions); 372 | for (size_t i{0}; i < arm_ids.size(); i++) 373 | printf("Current Position of ID: %03d is: %4d ticks or %6.3f radians.\n", arm_ids.at(i), positions.at(i), convertValue2Radian(positions.at(i))); 374 | 375 | // Write home and sleep positions to the arm joints in sequence 376 | std::vector home_positions(5,2048); 377 | std::vector sleep_positions = {2048, 803, 3099, 2570, 2048}; 378 | syncWrite(&groupSyncWrite, &arm_ids, &home_positions); 379 | sleep(2); 380 | syncWrite(&groupSyncWrite, &arm_ids, &sleep_positions); 381 | sleep(2); 382 | 383 | // Command the gripper to open for 2 seconds, then close for 2 seconds 384 | itemWrite(gripper_id, ADDR_GOAL_PWM, (int16_t)350); 385 | sleep(2); 386 | itemWrite(gripper_id, ADDR_GOAL_PWM, (int16_t)-350); 387 | sleep(2); 388 | itemWrite(gripper_id, ADDR_GOAL_PWM, (int16_t)0); 389 | 390 | // Read the present temperature of all the motors 391 | std::vector temps; 392 | itemReadMultiple(&all_ids, ADDR_PRESENT_TEMP, &temps); 393 | for (size_t i{0}; i < all_ids.size(); i++) 394 | printf("ID: %03d Temperature is: %d degrees Celsius.\n", all_ids.at(i), temps.at(i)); 395 | 396 | // Read the present loads/currents of all motors 397 | std::vector loads; 398 | itemReadMultiple(&all_ids, ADDR_PRESENT_LOAD, &loads); 399 | for (size_t i{0}; i < all_ids.size(); i++) 400 | printf("ID: %03d Raw Load/Current is: %d.\n", all_ids.at(i), loads.at(i)); 401 | 402 | portHandler->closePort(); 403 | return 0; 404 | } 405 | -------------------------------------------------------------------------------- /cpp/linux32/Makefile: -------------------------------------------------------------------------------- 1 | TARGET = interbotix_arm 2 | 3 | # important directories used by assorted rules and other variables 4 | DIR_DXL = /usr/local/ 5 | DIR_OBJS = .objects 6 | 7 | # compiler options 8 | CC = gcc 9 | CX = g++ 10 | CCFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall $(INCLUDES) $(FORMAT) -g 11 | CXFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall $(INCLUDES) $(FORMAT) -g -std=c++11 12 | LNKCC = $(CX) 13 | LNKFLAGS = $(CXFLAGS) #-Wl,-rpath,$(DIR_THOR)/lib 14 | FORMAT = -m32 15 | 16 | #--------------------------------------------------------------------- 17 | # Core components (all of these are likely going to be needed) 18 | #--------------------------------------------------------------------- 19 | INCLUDES += -I$(DIR_DXL)/include/dynamixel_sdk 20 | LIBRARIES += -ldxl_x86_cpp 21 | LIBRARIES += -lrt 22 | 23 | #--------------------------------------------------------------------- 24 | # Files 25 | #--------------------------------------------------------------------- 26 | SOURCES = interbotix_arm.cpp \ 27 | # *** OTHER SOURCES GO HERE *** 28 | 29 | OBJECTS = $(addsuffix .o,$(addprefix $(DIR_OBJS)/,$(basename $(notdir $(SOURCES))))) 30 | #OBJETCS += *** ADDITIONAL STATIC LIBRARIES GO HERE *** 31 | 32 | 33 | #--------------------------------------------------------------------- 34 | # Compiling Rules 35 | #--------------------------------------------------------------------- 36 | $(TARGET): make_directory $(OBJECTS) 37 | $(LNKCC) $(LNKFLAGS) $(OBJECTS) -o $(TARGET) $(LIBRARIES) 38 | 39 | all: $(TARGET) 40 | 41 | clean: 42 | rm -rf $(TARGET) $(DIR_OBJS) core *~ *.a *.so *.lo 43 | 44 | make_directory: 45 | mkdir -p $(DIR_OBJS)/ 46 | 47 | $(DIR_OBJS)/%.o: ../%.c 48 | $(CC) $(CCFLAGS) -c $? -o $@ 49 | 50 | $(DIR_OBJS)/%.o: ../%.cpp 51 | $(CX) $(CXFLAGS) -c $? -o $@ 52 | -------------------------------------------------------------------------------- /cpp/linux64/Makefile: -------------------------------------------------------------------------------- 1 | TARGET = interbotix_arm 2 | 3 | # important directories used by assorted rules and other variables 4 | DIR_DXL = /usr/local/ 5 | DIR_OBJS = .objects 6 | 7 | # compiler options 8 | CC = gcc 9 | CX = g++ 10 | CCFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall $(INCLUDES) $(FORMAT) -g 11 | CXFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall $(INCLUDES) $(FORMAT) -g -std=c++11 12 | LNKCC = $(CX) 13 | LNKFLAGS = $(CXFLAGS) #-Wl,-rpath,$(DIR_THOR)/lib 14 | FORMAT = -m64 15 | 16 | #--------------------------------------------------------------------- 17 | # Core components (all of these are likely going to be needed) 18 | #--------------------------------------------------------------------- 19 | INCLUDES += -I$(DIR_DXL)/include/dynamixel_sdk 20 | LIBRARIES += -ldxl_x64_cpp 21 | LIBRARIES += -lrt 22 | 23 | #--------------------------------------------------------------------- 24 | # Files 25 | #--------------------------------------------------------------------- 26 | SOURCES = interbotix_arm.cpp \ 27 | # *** OTHER SOURCES GO HERE *** 28 | 29 | OBJECTS = $(addsuffix .o,$(addprefix $(DIR_OBJS)/,$(basename $(notdir $(SOURCES))))) 30 | #OBJETCS += *** ADDITIONAL STATIC LIBRARIES GO HERE *** 31 | 32 | 33 | #--------------------------------------------------------------------- 34 | # Compiling Rules 35 | #--------------------------------------------------------------------- 36 | $(TARGET): make_directory $(OBJECTS) 37 | $(LNKCC) $(LNKFLAGS) $(OBJECTS) -o $(TARGET) $(LIBRARIES) 38 | 39 | all: $(TARGET) 40 | 41 | clean: 42 | rm -rf $(TARGET) $(DIR_OBJS) core *~ *.a *.so *.lo 43 | 44 | make_directory: 45 | mkdir -p $(DIR_OBJS)/ 46 | 47 | $(DIR_OBJS)/%.o: ../%.c 48 | $(CC) $(CCFLAGS) -c $? -o $@ 49 | 50 | $(DIR_OBJS)/%.o: ../%.cpp 51 | $(CX) $(CXFLAGS) -c $? -o $@ 52 | -------------------------------------------------------------------------------- /cpp/linux_sbc/Makefile: -------------------------------------------------------------------------------- 1 | TARGET = interbotix_arm 2 | 3 | # important directories used by assorted rules and other variables 4 | DIR_DXL = /usr/local 5 | DIR_OBJS = .objects 6 | 7 | # compiler options 8 | CC = gcc 9 | CX = g++ 10 | CCFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall $(INCLUDES) -g 11 | CXFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall $(INCLUDES) -g -std=c++11 12 | LNKCC = $(CX) 13 | LNKFLAGS = $(CXFLAGS) #-Wl,-rpath,$(DIR_THOR)/lib 14 | 15 | #--------------------------------------------------------------------- 16 | # Core components (all of these are likely going to be needed) 17 | #--------------------------------------------------------------------- 18 | INCLUDES += -I$(DIR_DXL)/include/dynamixel_sdk 19 | LIBRARIES += -ldxl_sbc_cpp 20 | LIBRARIES += -lrt 21 | 22 | #--------------------------------------------------------------------- 23 | # Files 24 | #--------------------------------------------------------------------- 25 | SOURCES = interbotix_arm.cpp \ 26 | # *** OTHER SOURCES GO HERE *** 27 | 28 | OBJECTS = $(addsuffix .o,$(addprefix $(DIR_OBJS)/,$(basename $(notdir $(SOURCES))))) 29 | #OBJETCS += *** ADDITIONAL STATIC LIBRARIES GO HERE *** 30 | 31 | 32 | #--------------------------------------------------------------------- 33 | # Compiling Rules 34 | #--------------------------------------------------------------------- 35 | $(TARGET): make_directory $(OBJECTS) 36 | $(LNKCC) $(LNKFLAGS) $(OBJECTS) -o $(TARGET) $(LIBRARIES) 37 | 38 | all: $(TARGET) 39 | 40 | clean: 41 | rm -rf $(TARGET) $(DIR_OBJS) core *~ *.a *.so *.lo 42 | 43 | make_directory: 44 | mkdir -p $(DIR_OBJS)/ 45 | 46 | $(DIR_OBJS)/%.o: ../%.c 47 | $(CC) $(CCFLAGS) -c $? -o $@ 48 | 49 | $(DIR_OBJS)/%.o: ../%.cpp 50 | $(CX) $(CXFLAGS) -c $? -o $@ 51 | -------------------------------------------------------------------------------- /cpp/mac/Makefile: -------------------------------------------------------------------------------- 1 | TARGET = interbotix_arm 2 | 3 | # important directories used by assorted rules and other variables 4 | DIR_DXL = /usr/local/ 5 | DIR_OBJS = .objects 6 | 7 | # compiler options 8 | CC = gcc 9 | CX = g++ 10 | CCFLAGS = -O2 -O3 -D_GNU_SOURCE -Wall $(INCLUDES) -g 11 | CXFLAGS = -O2 -O3 -D_GNU_SOURCE -Wall $(INCLUDES) -g -std=c++11 12 | LNKCC = $(CX) 13 | LNKFLAGS = $(CXFLAGS) #-Wl,-rpath,$(DIR_THOR)/lib 14 | 15 | #--------------------------------------------------------------------- 16 | # Core components (all of these are likely going to be needed) 17 | #--------------------------------------------------------------------- 18 | INCLUDES += -I$(DIR_DXL)/include/dynamixel_sdk 19 | LIBRARIES += -ldxl_mac_cpp 20 | 21 | #--------------------------------------------------------------------- 22 | # Files 23 | #--------------------------------------------------------------------- 24 | SOURCES = interbotix_arm.cpp \ 25 | # *** OTHER SOURCES GO HERE *** 26 | 27 | OBJECTS = $(addsuffix .o,$(addprefix $(DIR_OBJS)/,$(basename $(notdir $(SOURCES))))) 28 | #OBJETCS += *** ADDITIONAL STATIC LIBRARIES GO HERE *** 29 | 30 | 31 | #--------------------------------------------------------------------- 32 | # Compiling Rules 33 | #--------------------------------------------------------------------- 34 | $(TARGET): make_directory $(OBJECTS) 35 | $(LNKCC) $(LNKFLAGS) $(OBJECTS) -o $(TARGET) $(LIBRARIES) 36 | 37 | all: $(TARGET) 38 | 39 | clean: 40 | rm -rf $(TARGET) $(DIR_OBJS) core *~ *.a *.so *.lo 41 | 42 | make_directory: 43 | mkdir -p $(DIR_OBJS)/ 44 | 45 | $(DIR_OBJS)/%.o: ../%.c 46 | $(CC) $(CCFLAGS) -c $? -o $@ 47 | 48 | $(DIR_OBJS)/%.o: ../%.cpp 49 | $(CX) $(CXFLAGS) -c $? -o $@ 50 | -------------------------------------------------------------------------------- /python/interbotix_arm.py: -------------------------------------------------------------------------------- 1 | import time 2 | from dynamixel_sdk import * 3 | 4 | ADDR_DRIVE_MODE = 10 5 | ADDR_OPERATING_MODE = 11 6 | ADDR_SECONDARY_ID = 12 7 | ADDR_HOMING_OFFSET = 20 8 | ADDR_MAX_POS_LIMIT = 48 9 | ADDR_MIN_POS_LIMIT = 52 10 | ADDR_TORQUE_ENABLE = 64 11 | ADDR_POSITION_P_GAIN = 84 12 | ADDR_GOAL_PWM = 100 13 | ADDR_PROFILE_ACCELERATION = 108 14 | ADDR_PROFILE_VELOCITY = 112 15 | ADDR_GOAL_POSITION = 116 16 | ADDR_PRESENT_LOAD = 126 17 | ADDR_PRESENT_POSITION = 132 18 | ADDR_PRESENT_TEMP = 146 19 | 20 | LEN_DRIVE_MODE = 1 21 | LEN_OPERATING_MODE = 1 22 | LEN_SECONDARY_ID = 1 23 | LEN_HOMING_OFFSET = 4 24 | LEN_MAX_POS_LIMIT = 4 25 | LEN_MIN_POS_LIMIT = 4 26 | LEN_TORQUE_ENABLE = 1 27 | LEN_POSITION_P_GAIN = 2 28 | LEN_GOAL_PWM = 2 29 | LEN_PROFILE_ACCELERATION = 4 30 | LEN_PROFILE_VELOCITY = 4 31 | LEN_GOAL_POSITION = 4 32 | LEN_PRESENT_LOAD = 2 33 | LEN_PRESENT_POSITION = 4 34 | LEN_PRESENT_TEMP = 1 35 | 36 | PI = 3.14159265 37 | 38 | portHandler = None 39 | packetHandler = None 40 | 41 | ### @brief Helper function to check error messages and print them if need be 42 | ### @param dxl_comm_result - if nonzero, there is an error in the communication 43 | ### @param dxl_error - if nonzero, there is an error in something related to the data 44 | ### @return - true if no error occurred; false otherwise 45 | def checkError(dxl_comm_result, dxl_error): 46 | if dxl_comm_result != COMM_SUCCESS: 47 | print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) 48 | return False 49 | elif dxl_error != 0: 50 | print("%s" % packetHandler.getRxPacketError(dxl_error)) 51 | return False 52 | return True 53 | 54 | ### @brief Writes data to the specified register for a given motor 55 | ### @param id - Dynamixel ID to write data to 56 | ### @param address - register number 57 | ### @param data - data to write 58 | ### @param length - size of register in bytes 59 | ### @return - true if data was successfully written; false otherwise 60 | def itemWrite(id, address, data, length): 61 | if length == 1: 62 | dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, id, address, data) 63 | elif length == 2: 64 | dxl_comm_result, dxl_error = packetHandler.write2ByteTxRx(portHandler, id, address, data) 65 | elif length == 4: 66 | dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, id, address, data) 67 | else: 68 | print("Invalid data length...") 69 | return False 70 | return checkError(dxl_comm_result, dxl_error) 71 | 72 | ### @brief Sequentially writes the same data to the specified register for a group of motors 73 | ### @param ids - vector of IDs to write data to 74 | ### @param address - register number 75 | ### @param data - data to write (could be a list [index matches index in 'ids' list] or a single value [for all ids]) 76 | ### @param length - size of register in bytes 77 | ### @return - true if data was successfully written; false otherwise 78 | def itemWriteMultiple(ids, address, data, length): 79 | if type(data) != list: 80 | for id in ids: 81 | success = itemWrite(id, address, data, length) 82 | if success != True: 83 | return False 84 | else: 85 | for id, dat in zip(ids, data): 86 | success = itemWrite(id, address, dat, length) 87 | if success != True: 88 | return False 89 | return True 90 | 91 | ### @brief Reads data from the specified register for a given motor 92 | ### @param id - Dynamixel ID to read data from 93 | ### @param address - register number 94 | ### @param length - size of register in bytes 95 | ### @return state - variable to store the requested data 96 | ### @return - true if data was successfully retrieved; false otherwise 97 | ### @details - DynamixelSDK uses 2's complement so we need to check to see if 'state' should be negative (hex numbers) 98 | def itemRead(id, address, length): 99 | if length == 1: 100 | state, dxl_comm_result, dxl_error = packetHandler.read1ByteTxRx(portHandler, id, address) 101 | elif length == 2: 102 | state, dxl_comm_result, dxl_error = packetHandler.read2ByteTxRx(portHandler, id, address) 103 | if state > 0x7fff: 104 | state = state - 65536 105 | elif length == 4: 106 | state, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, id, address) 107 | if state > 0x7fffffff: 108 | state = state - 4294967296 109 | else: 110 | print("Invalid data length...") 111 | return 0, False 112 | return state, checkError(dxl_comm_result, dxl_error) 113 | 114 | ### @brief Sequentially reads data from the specified register for a group of motors 115 | ### @param id - vector of Dynamixel IDs to read data from 116 | ### @param address - register number 117 | ### @param length - size of register in bytes 118 | ### @return states - list to store the requested data 119 | ### @return - true if data was successfully retrieved; false otherwise 120 | def itemReadMultiple(ids, address, length): 121 | states = [] 122 | for id in ids: 123 | state, success = itemRead(id, address, length) 124 | if success != True: 125 | return [], False 126 | states.append(state) 127 | return states, True 128 | 129 | ### @brief Writes data to a group of motors synchronously 130 | ### @param groupSyncWrite - groupSyncWrite object 131 | ### @param ids - list of Dynamixel IDs to write to 132 | ### @param commands - list of commands to write to each respective motor 133 | ### @param length - size of register in bytes 134 | ### @return - true if data was successfully written; false otherwise 135 | def syncWrite(groupSyncWrite, ids, commands, length): 136 | groupSyncWrite.clearParam() 137 | for id, cmd in zip(ids, commands): 138 | param = [] 139 | if length == 4: 140 | param.append(DXL_LOBYTE(DXL_LOWORD(cmd))) 141 | param.append(DXL_HIBYTE(DXL_LOWORD(cmd))) 142 | param.append(DXL_LOBYTE(DXL_HIWORD(cmd))) 143 | param.append(DXL_HIBYTE(DXL_HIWORD(cmd))) 144 | elif length == 2: 145 | param.append(DXL_LOBYTE(cmd)) 146 | param.append(DXL_HIBYTE(cmd)) 147 | else: 148 | param.append(cmd) 149 | dxl_addparam_result = groupSyncWrite.addParam(id, param) 150 | if dxl_addparam_result != True: 151 | print("ID:%03d groupSyncWrite addparam failed" % id) 152 | return False 153 | dxl_comm_result = groupSyncWrite.txPacket() 154 | if dxl_comm_result != COMM_SUCCESS: 155 | print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) 156 | return False 157 | return True 158 | 159 | ### @brief Reads data from a group of motors synchronously 160 | ### @param groupSyncRead - groupSyncRead object 161 | ### @param ids - list of Dynamixel IDs to read from 162 | ### @param address - register number 163 | ### @param length - size of register in bytes 164 | ### @return states - list to store the requested data 165 | ### @return - true if data was successfully retrieved; false otherwise 166 | ### @details - DynamixelSDK uses 2's complement so we need to check to see if 'state' should be negative (hex numbers) 167 | def syncRead(groupSyncRead, ids, address, length): 168 | groupSyncRead.clearParam() 169 | for id in ids: 170 | dxl_addparam_result = groupSyncRead.addParam(id) 171 | if dxl_addparam_result != True: 172 | print("ID:%03d groupSyncRead addparam failed", id) 173 | return [], False 174 | 175 | dxl_comm_result = groupSyncRead.txRxPacket() 176 | if dxl_comm_result != COMM_SUCCESS: 177 | print("%s" % packetHandler.getTxRxResult(dxl_comm_result)) 178 | return [], False 179 | 180 | states = [] 181 | for id in ids: 182 | state = groupSyncRead.getData(id, address, length) 183 | if length == 2 and state > 0x7fff: 184 | state = state - 65536 185 | elif length == 4 and state > 0x7fffffff: 186 | state = state - 4294967296 187 | states.append(state) 188 | return states, True 189 | 190 | ### @brief Calibrates a dual-motor joint so that both motors show the same Present Position value 191 | ### @param master_id - in a dual-motor joint setup, this is the ID with the lower value 192 | ### @param slave_id - in a dual-motor joint setup, this is the ID with the higher value 193 | ### @details - note that the Drive mode must be set beforehand for both motors (specifically if CW or CCW is positive) 194 | def calibrateDualJoint(master_id, slave_id): 195 | itemWrite(slave_id, ADDR_HOMING_OFFSET, 0, LEN_HOMING_OFFSET) 196 | master_position,_ = itemRead(master_id, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION) 197 | slave_position,_ = itemRead(slave_id, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION) 198 | slave_drive_mode,_ = itemRead(slave_id, ADDR_DRIVE_MODE, LEN_DRIVE_MODE) 199 | slave_forward = (slave_drive_mode % 2 == 0) 200 | print("Master Position: %d" % master_position) 201 | print("Slave Position: %d" % slave_position) 202 | if slave_forward: 203 | homing_offset = master_position - slave_position 204 | else: 205 | homing_offset = slave_position - master_position 206 | print("Setting Slave Homing Offset Register to: %d" % homing_offset) 207 | itemWrite(slave_id, ADDR_HOMING_OFFSET, homing_offset, LEN_HOMING_OFFSET) 208 | 209 | ### @brief Initializes the port that the U2D2 is connected to 210 | ### @param port_name - name of the port 211 | ### @baudrate - desired baudrate in bps (should be the same as the motors) 212 | ### @return - true if the port was initialized; false otherwise 213 | def initPort(port_name, baudrate): 214 | global portHandler, packetHandler 215 | portHandler = PortHandler(port_name) 216 | packetHandler = PacketHandler(2.0) 217 | 218 | if portHandler.openPort(): 219 | print("Successfully opened the port at %s!" % port_name) 220 | else: 221 | print("Failed to open the port at %s!", port_name) 222 | return False 223 | 224 | if portHandler.setBaudRate(baudrate): 225 | print("Succeeded to change the baudrate to %d bps!" % baudrate) 226 | else: 227 | print("Failed to change the baudrate to %d bps!" % baudrate) 228 | return False 229 | 230 | return True 231 | 232 | ### @brief Ping the desired motors to verify their existence 233 | ### @param ids - vector of Dynamixel IDs to ping 234 | ### @return - true if all IDs were pinged successfully 235 | def ping(ids): 236 | for id in ids: 237 | model_num, dxl_comm_result, dxl_error = packetHandler.ping(portHandler, id) 238 | success = checkError(dxl_comm_result, dxl_error) 239 | if success: 240 | print("Pinged ID: %03d successfully! Model Number: %d" % (id,model_num)) 241 | else: 242 | return False 243 | return True 244 | 245 | ### @brief Convert a raw Position register value into radians 246 | ### @param value - desired raw value to convert 247 | ### @return - value in radians 248 | def convertValue2Radian(value): 249 | return (value - 2047.5)/2047.5 * PI 250 | 251 | ### @brief Convert a value in radians to the raw Position register value 252 | ### @param radian - desired value in radians to convert 253 | ### @return - raw register value 254 | def convertRadian2Value(radian): 255 | return round(radian/PI * 2047.5 + 2047.5) 256 | 257 | def main(): 258 | ## WX200 ARM EEPROM CONFIGS 259 | all_ids = [1,2,3,4,5,6,7] 260 | drive_modes = [4,4,5,5,5,4,4] 261 | shadow_ids = [255,255,2,255,255,255,255] 262 | min_position_limits = [0,0,0,0,0,0,0] 263 | max_position_limits = [4095,4095,4095,4095,4095,4095,4095] 264 | operating_modes = [3,3,3,3,3,3,16] 265 | position_p_gains = [800, 1500, 2000, 1000, 640] 266 | 267 | ## WX200 ARM 'Command' IDs 268 | arm_ids = [1,2,4,5,6] 269 | gripper_id = 7 270 | shoulder_master_id = 2 271 | shoulder_slave_id = 3 272 | 273 | ## Initialize the port, ping the motors, and create syncWrite and syncRead objects 274 | ## It's faster and better design to read/write motors with the 'sync' objects than to command each motor sequentially 275 | ## Commanding each motor sequentially should only be done for 'non-realtime sensitive' registers - like torquing on/off, setting EEPROM registers, etc... 276 | if not initPort("/dev/ttyDXL", 1000000): 277 | return 278 | if not ping(all_ids): 279 | portHandler.closePort() 280 | return 281 | groupSyncWrite = GroupSyncWrite(portHandler, packetHandler, ADDR_GOAL_POSITION, LEN_GOAL_POSITION) 282 | groupSyncRead = GroupSyncRead(portHandler, packetHandler, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION) 283 | 284 | ## Initialize register values 285 | itemWriteMultiple(all_ids, ADDR_TORQUE_ENABLE, 0, LEN_TORQUE_ENABLE) 286 | itemWriteMultiple(all_ids, ADDR_DRIVE_MODE, drive_modes, LEN_DRIVE_MODE) 287 | itemWriteMultiple(all_ids, ADDR_SECONDARY_ID, shadow_ids, LEN_SECONDARY_ID) 288 | itemWriteMultiple(all_ids, ADDR_MIN_POS_LIMIT, min_position_limits, LEN_MIN_POS_LIMIT) 289 | itemWriteMultiple(all_ids, ADDR_MAX_POS_LIMIT, max_position_limits, LEN_MAX_POS_LIMIT) 290 | itemWriteMultiple(all_ids, ADDR_OPERATING_MODE, operating_modes, LEN_OPERATING_MODE) 291 | itemWriteMultiple(arm_ids, ADDR_POSITION_P_GAIN, position_p_gains, LEN_POSITION_P_GAIN) 292 | itemWriteMultiple(arm_ids, ADDR_PROFILE_VELOCITY, 1500, LEN_PROFILE_VELOCITY) 293 | itemWriteMultiple(arm_ids, ADDR_PROFILE_ACCELERATION, 750, LEN_PROFILE_ACCELERATION) 294 | calibrateDualJoint(shoulder_master_id, shoulder_slave_id) 295 | itemWriteMultiple(all_ids, ADDR_TORQUE_ENABLE, 1, LEN_TORQUE_ENABLE) 296 | 297 | ## Read current arm joint positions 298 | positions, success = syncRead(groupSyncRead, arm_ids, ADDR_PRESENT_POSITION, LEN_PRESENT_POSITION) 299 | print(positions) 300 | 301 | ## Write home and sleep positions to the arm joints in sequence 302 | home_positions = [2048] * 5 303 | sleep_positions = [2048, 803, 3099, 2570, 2048] 304 | syncWrite(groupSyncWrite, arm_ids, home_positions, LEN_GOAL_POSITION) 305 | time.sleep(2) 306 | syncWrite(groupSyncWrite, arm_ids, sleep_positions, LEN_GOAL_POSITION) 307 | time.sleep(2) 308 | 309 | ## Command the gripper to open for 2 seconds, then close for 2 seconds 310 | itemWrite(gripper_id, ADDR_GOAL_PWM, 350, LEN_GOAL_PWM) 311 | time.sleep(2) 312 | itemWrite(gripper_id, ADDR_GOAL_PWM, -350, LEN_GOAL_PWM) 313 | time.sleep(2) 314 | itemWrite(gripper_id, ADDR_GOAL_PWM, 0, LEN_GOAL_PWM) 315 | 316 | ## Read the present temperature of all the motors 317 | temps, success = itemReadMultiple(all_ids, ADDR_PRESENT_TEMP, LEN_PRESENT_TEMP) 318 | for id, temp in zip(all_ids, temps): 319 | print("ID: %03d Temperature is: %d degrees Celsius." % (id, temp)) 320 | 321 | ## Read the present loads/currents of all motors 322 | loads, success = itemReadMultiple(all_ids, ADDR_PRESENT_LOAD, LEN_PRESENT_LOAD) 323 | print(loads) 324 | 325 | portHandler.closePort() 326 | 327 | if __name__=='__main__': 328 | main() 329 | --------------------------------------------------------------------------------