├── config
    ├── xArm-49770F673737-gripper.json
    └── xArm-49770F673737-arm.json
├── setup.py
├── package.xml
├── launch
    └── display.launch
├── src
    └── lewansoul_xarm
    │   ├── __init__.py
    │   ├── controller_ros.py
    │   ├── arm_ros.py
    │   ├── controller.py
    │   └── arm.py
├── LICENSE
├── CMakeLists.txt
├── scripts
    └── xArm-ros.py
├── rviz
    ├── urdf.vcg
    └── urdf.rviz
├── README.md
└── urdf
    └── xArm.urdf


/config/xArm-49770F673737-gripper.json:
--------------------------------------------------------------------------------
 1 | {
 2 |     "controller-serial-number": "49770F673737", 
 3 |     "directions": [
 4 |         -1.0
 5 |     ], 
 6 |     "offsets": [
 7 |         2.810678227411668
 8 |     ], 
 9 |     "servo-ids": [
10 |         1
11 |     ]
12 | }


--------------------------------------------------------------------------------
/setup.py:
--------------------------------------------------------------------------------
 1 |  ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
 2 | from distutils.core import setup
 3 | from catkin_pkg.python_setup import generate_distutils_setup
 4 | 
 5 | # fetch values from package.xml
 6 | setup_args = generate_distutils_setup(packages=['lewansoul_xarm'],
 7 |                                       package_dir={'': 'src'},
 8 |                                       )
 9 | 
10 | setup(**setup_args)
11 | 


--------------------------------------------------------------------------------
/config/xArm-49770F673737-arm.json:
--------------------------------------------------------------------------------
 1 | {
 2 |     "controller-serial-number": "49770F673737", 
 3 |     "directions": [
 4 |         1.0, 
 5 |         1.0, 
 6 |         -1.0, 
 7 |         1.0, 
 8 |         1.0
 9 |     ], 
10 |     "offsets": [
11 |         -2.22843638894636, 
12 |         -2.1069614730075545, 
13 |         2.0943951023931953, 
14 |         -2.1823596966937093, 
15 |         -2.165604535874564
16 |     ], 
17 |     "servo-ids": [
18 |         6, 
19 |         5, 
20 |         4, 
21 |         3, 
22 |         2
23 |     ]
24 | }


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>lewansoul_xarm</name>
 4 |   <version>0.0.0</version>
 5 |   <description>Python class to communicate with LewanSoul xArm over HID</description>
 6 | 
 7 |   <maintainer email="anton.deguet@jhu.edu">Anton Deguet</maintainer>
 8 |   <license>CISST</license>
 9 | 
10 |   <buildtool_depend>catkin</buildtool_depend>
11 |   <build_depend>rospy</build_depend>
12 |   <run_depend>rospy</run_depend>
13 | 
14 |   <!-- The export tag contains other, unspecified, tags -->
15 |   <export>
16 |   </export>
17 | 
18 | </package>
19 | 


--------------------------------------------------------------------------------
/launch/display.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 | 
 3 |   <arg name="model" default="$(find lewansoul_xarm)/urdf/xArm.urdf"/>
 4 |   <arg name="rvizconfig" default="$(find lewansoul_xarm)/rviz/urdf.rviz" />
 5 | 
 6 |   <param name="robot_description" command="$(find xacro)/xacro.py $(arg model)" />
 7 | 
 8 |   <rosparam param="source_list" subst_value="True">
 9 |     [/arm/measured_js]
10 |   </rosparam>
11 | 
12 |   <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
13 |   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
14 |   <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
15 | 
16 | </launch>
17 | 


--------------------------------------------------------------------------------
/src/lewansoul_xarm/__init__.py:
--------------------------------------------------------------------------------
 1 | #  Author(s):  Anton Deguet
 2 | #  Created on: 2020-04
 3 | 
 4 | #   (C) Copyright 2020 Johns Hopkins University (JHU), All Rights Reserved.
 5 | 
 6 | # --- begin cisst license - do not edit ---
 7 | 
 8 | # This software is provided "as is" under an open source license, with
 9 | # no warranty.  The complete license can be found in license.txt and
10 | # http://www.cisst.org/cisst/license.txt.
11 | 
12 | # --- end cisst license ---
13 | 
14 | __all__ = ["arm", "arm_ros", "controller", "controller_ros"]
15 | 
16 | # main arm class
17 | from .arm import arm
18 | from .arm_ros import arm_ros
19 | from .controller import controller
20 | from .controller_ros import controller_ros
21 | 


--------------------------------------------------------------------------------
/src/lewansoul_xarm/controller_ros.py:
--------------------------------------------------------------------------------
 1 | #  Author(s):  Anton Deguet
 2 | #  Created on: 2020-04
 3 | 
 4 | # (C) Copyright 2020 Johns Hopkins University (JHU), All Rights Reserved.
 5 | 
 6 | # --- begin cisst license - do not edit ---
 7 | 
 8 | # This software is provided "as is" under an open source license, with
 9 | # no warranty.  The complete license can be found in license.txt and
10 | # http://www.cisst.org/cisst/license.txt.
11 | 
12 | # --- end cisst license ---
13 | 
14 | import rospy
15 | import lewansoul_xarm
16 | 
17 | 
18 | class controller_ros(object):
19 |     """Simple arm API wrapping around ROS messages
20 |     """
21 | 
22 |     # initialize the controller
23 |     def __init__(self, controller):
24 |         self._controller = controller
25 |         self._arms = []
26 |         for arm in self._controller._arms:
27 |             self._arms.append(lewansoul_xarm.arm_ros(arm))
28 | 
29 | 
30 |     def loop(self, rate):
31 |         ros_rate = rospy.Rate(rate)
32 |         while not rospy.is_shutdown():
33 |             for arm in self._arms:
34 |                 arm.publish()
35 |             ros_rate.sleep()
36 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2020 Anton Deguet
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required (VERSION 2.8.3)
 2 | project (lewansoul_xarm)
 3 | 
 4 | ## Find catkin macros and libraries
 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
 6 | ## is used, also find other catkin packages
 7 | find_package (catkin REQUIRED COMPONENTS
 8 |               rospy
 9 |               )
10 | 
11 | ## The catkin_package macro generates cmake config files for your package
12 | ## Declare things to be passed to dependent projects
13 | ## INCLUDE_DIRS: uncomment this if you package contains header files
14 | ## LIBRARIES: libraries you create in this project that dependent projects also need
15 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need
16 | ## DEPENDS: system dependencies of this project that dependent projects also need
17 | catkin_package (
18 | #  INCLUDE_DIRS include
19 | #  LIBRARIES my_pkg
20 |    CATKIN_DEPENDS rospy
21 | #  DEPENDS system_lib
22 | )
23 | 
24 | ## Mark executable scripts (Python etc.) for installation
25 | ## in contrast to setup.py, you can choose the destination
26 | # install(PROGRAMS
27 | #   scripts/my_python_script
28 | #   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
29 | # )
30 | 
31 | catkin_python_setup ()
32 | 


--------------------------------------------------------------------------------
/scripts/xArm-ros.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python
 2 | 
 3 | import os
 4 | import rospy
 5 | import rospkg
 6 | import lewansoul_xarm
 7 | 
 8 | def shutdown():
 9 |     print('\nshutting down xArm...')
10 |     global controller
11 |     controller.shutdown()
12 | 
13 | def ros_main():
14 |     # connect to xArm
15 |     global controller # so we can shutdown
16 |     controller = lewansoul_xarm.controller()
17 | 
18 |     rospack = rospkg.RosPack()
19 |     pkg_path = rospack.get_path('lewansoul_xarm')
20 |     
21 |     urdf_file = pkg_path + '/urdf/xArm.urdf'
22 |     if not os.path.isfile(urdf_file):
23 |         print('couldn\'t find file: ' + urdf_file + '. cartesian moves will be disabled')
24 |         urdf_file = ''
25 |     arm = controller.add_arm("arm", "xArm-49770F673737-arm.json", urdf_file)
26 |     arm.enable()
27 |     arm.home()
28 | 
29 |     gripper = controller.add_arm("gripper", "xArm-49770F673737-gripper.json")
30 |     gripper.enable()
31 |     gripper.home()
32 | 
33 |     rospy.init_node('xArm_ros', anonymous = True)
34 |     rospy.on_shutdown(shutdown)
35 | 
36 |     controller_ros = lewansoul_xarm.controller_ros(controller)
37 |     controller_ros.loop(30) # 30 Hz seems to be the maximum the controller can perform
38 | 
39 | 
40 | if __name__ == '__main__':
41 |     try:
42 |         ros_main()
43 |     except rospy.ROSInterruptException:
44 |         pass
45 | 


--------------------------------------------------------------------------------
/rviz/urdf.vcg:
--------------------------------------------------------------------------------
 1 | Background\ ColorR=0.0941176
 2 | Background\ ColorG=0
 3 | Background\ ColorB=0.466667
 4 | Fixed\ Frame=/base_link
 5 | Target\ Frame=<Fixed Frame>
 6 | Grid.Alpha=0.5
 7 | Grid.Cell\ Size=0.5
 8 | Grid.ColorR=0.898039
 9 | Grid.ColorG=0.898039
10 | Grid.ColorB=0.898039
11 | Grid.Enabled=1
12 | Grid.Line\ Style=0
13 | Grid.Line\ Width=0.03
14 | Grid.Normal\ Cell\ Count=0
15 | Grid.OffsetX=0
16 | Grid.OffsetY=0
17 | Grid.OffsetZ=0
18 | Grid.Plane=0
19 | Grid.Plane\ Cell\ Count=10
20 | Grid.Reference\ Frame=<Fixed Frame>
21 | Robot\ Model.Alpha=1
22 | Robot\ Model.Collision\ Enabled=0
23 | Robot\ Model.Enabled=1
24 | Robot\ Model.Robot\ Description=robot_description
25 | Robot\ Model.TF\ Prefix=
26 | Robot\ Model.Update\ Interval=0
27 | Robot\ Model.Visual\ Enabled=1
28 | Robot\:\ Robot\ Model\ Link\ base_linkShow\ Axes=0
29 | Robot\:\ Robot\ Model\ Link\ base_linkShow\ Trail=0
30 | Robot\:\ Robot\ Model\ Link\ link1\ Axes=1
31 | Robot\:\ Robot\ Model\ Link\ link1\ Trail=0
32 | Robot\:\ Robot\ Model\ Link\ link2\ Axes=1
33 | Robot\:\ Robot\ Model\ Link\ link2\ Trail=0
34 | Robot\:\ Robot\ Model\ Link\ link3\ Axes=1
35 | Robot\:\ Robot\ Model\ Link\ link3\ Trail=0
36 | Robot\:\ Robot\ Model\ Link\ link4\ Axes=1
37 | Robot\:\ Robot\ Model\ Link\ link4\ Trail=0
38 | Robot\:\ Robot\ Model\ Link\ link5\ Axes=1
39 | Robot\:\ Robot\ Model\ Link\ link5\ Trail=0
40 | Tool\ 2D\ Nav\ GoalTopic=goal
41 | Tool\ 2D\ Pose\ EstimateTopic=initialpose
42 | Camera\ Type=rviz::OrbitViewController
43 | Camera\ Config=1.15779 3.76081 2.16475 0 0 0
44 | Property\ Grid\ State=selection=.Global StatusTopStatus;expanded=.Global Options,Grid.Enabled,Robot Model.Enabled;scrollpos=0,0;splitterpos=150,285;ispageselected=1
45 | [Display0]
46 | Name=Grid
47 | Package=rviz
48 | ClassName=rviz::GridDisplay
49 | [Display1]
50 | Name=Robot Model
51 | Package=rviz
52 | ClassName=rviz::RobotModelDisplay
53 | 


--------------------------------------------------------------------------------
/src/lewansoul_xarm/arm_ros.py:
--------------------------------------------------------------------------------
 1 | #  Author(s):  Anton Deguet
 2 | #  Created on: 2020-04
 3 | 
 4 | # (C) Copyright 2020 Johns Hopkins University (JHU), All Rights Reserved.
 5 | 
 6 | # --- begin cisst license - do not edit ---
 7 | 
 8 | # This software is provided "as is" under an open source license, with
 9 | # no warranty.  The complete license can be found in license.txt and
10 | # http://www.cisst.org/cisst/license.txt.
11 | 
12 | # --- end cisst license ---
13 | 
14 | """
15 | 
16 | """
17 | 
18 | import numpy
19 | import rospy
20 | import std_msgs.msg
21 | import sensor_msgs.msg
22 | import geometry_msgs.msg
23 | 
24 | class arm_ros(object):
25 |     """
26 |     """
27 | 
28 |     def __init__(self, arm):
29 |         """
30 |         """
31 |         self._arm = arm
32 | 
33 |         ns = self._arm._name + '/'
34 |         # create publishers and corresponding message
35 |         # joint space
36 |         self._js_msg = sensor_msgs.msg.JointState()
37 |         for i in range(len(self._arm._servo_ids)):
38 |             self._js_msg.name.append(self._arm._name + str(i + 1))
39 |         self._measured_js_publisher = rospy.Publisher(ns + 'measured_js',
40 |                                                       sensor_msgs.msg.JointState,
41 |                                                       queue_size = 10)
42 |         self._goal_js_publisher = rospy.Publisher(ns + 'goal_js',
43 |                                                   sensor_msgs.msg.JointState,
44 |                                                   queue_size = 10)
45 |         # cartesian space
46 |         if self._arm._has_kin:
47 |             self._cp_msg = geometry_msgs.msg.TransformStamped()
48 |             self._measured_cp_publisher = rospy.Publisher(ns + 'measured_cp',
49 |                                                           geometry_msgs.msg.TransformStamped,
50 |                                                           queue_size = 10)
51 | 
52 |         # create subscribers
53 |         rospy.Subscriber(ns + "enable", std_msgs.msg.Empty, self.enable)
54 |         rospy.Subscriber(ns + "disable", std_msgs.msg.Empty, self.disable)
55 |         rospy.Subscriber(ns + "home", std_msgs.msg.Empty, self.home)
56 |         rospy.Subscriber(ns + "calibrate", std_msgs.msg.Empty, self.calibrate)
57 |         rospy.Subscriber(ns + "move_jp", sensor_msgs.msg.JointState, self.move_jp)
58 | 
59 |     def publish(self):
60 |         try:
61 |             # update data from controller
62 |             self._arm.get_data()
63 |             # measured joint state
64 |             measured_jp = self._arm.measured_jp()
65 |             self._js_msg.position[:] = measured_jp.flat
66 |             self._measured_js_publisher.publish(self._js_msg)
67 |             # goal joint position
68 |             goal_jp = self._arm.goal_jp()
69 |             self._js_msg.position[:] = goal_jp.flat
70 |             self._goal_js_publisher.publish(self._js_msg)
71 |             # measured cartesian position
72 |             if self._arm._has_kin:
73 |                 measured_cp = self._arm.measured_cp()
74 |                 t = self._cp_msg.transform
75 |                 t.rotation.x, t.rotation.y, t.rotation.z, t.rotation.w = measured_cp.M.GetQuaternion()
76 |                 t.translation.x = measured_cp.p[0]
77 |                 t.translation.y = measured_cp.p[1]
78 |                 t.translation.z = measured_cp.p[2]
79 |                 self._measured_cp_publisher.publish(self._cp_msg)
80 |         except:
81 |             print('failed to communicate with HID device, arm might be shutting down or controller has been turned off')
82 |             rospy.signal_shutdown('HID communication error')
83 | 
84 |     def enable(self, msg):
85 |         self._arm.enable()
86 | 
87 |     def disable(self, msg):
88 |         self._arm.disable()
89 | 
90 |     def home(self, msg):
91 |         self._arm.home()
92 | 
93 |     def calibrate(self, msg):
94 |         self._arm.calibrate()
95 | 
96 |     def move_jp(self, msg):
97 |         goal = numpy.array(msg.position)
98 |         self._arm.move_jp(goal)
99 | 


--------------------------------------------------------------------------------
/src/lewansoul_xarm/controller.py:
--------------------------------------------------------------------------------
  1 | #  Author(s):  Anton Deguet
  2 | #  Created on: 2020-04
  3 | 
  4 | # (C) Copyright 2020 Johns Hopkins University (JHU), All Rights Reserved.
  5 | 
  6 | # --- begin cisst license - do not edit ---
  7 | 
  8 | # This software is provided "as is" under an open source license, with
  9 | # no warranty.  The complete license can be found in license.txt and
 10 | # http://www.cisst.org/cisst/license.txt.
 11 | 
 12 | # --- end cisst license ---
 13 | 
 14 | """
 15 | 
 16 | Code based on:
 17 | https://gist.github.com/maximecb/7fd42439e8a28b9a74a4f7db68281071
 18 | https://github.com/maximkulkin/lewansoul-lx16a
 19 | 
 20 | sudo apt-get install libhidapi-hidraw0 libhidapi-libusb0 python-hid
 21 | pip install --user easyhid
 22 | """
 23 | CMD_SERVO_MOVE = 3
 24 | CMD_GET_BATTERY_VOLTAGE = 15
 25 | CMD_MULT_SERVO_UNLOAD = 20
 26 | CMD_MULT_SERVO_POS_READ = 21
 27 | 
 28 | import math
 29 | import easyhid
 30 | import numpy
 31 | import itertools
 32 | import threading
 33 | import lewansoul_xarm
 34 | 
 35 | def integer_to_bits(v):
 36 |     lsb = v & 0xFF
 37 |     msb = v >> 8
 38 |     return lsb, msb
 39 | 
 40 | class controller(object):
 41 |     """Simple arm API wrapping around ROS messages
 42 |     """
 43 | 
 44 |     # initialize the controller
 45 |     def __init__(self, pid = 22352):
 46 |         """
 47 |         """
 48 |         # stores an enumeration of all the connected USB HID devices
 49 |         en = easyhid.Enumeration()
 50 | 
 51 |         # return a list of devices based on the search parameters
 52 |         devices = en.find(vid = 1155, pid = pid)
 53 | 
 54 |         # print a description of the devices found
 55 |         for dev in devices:
 56 |             print(dev.description())
 57 | 
 58 |         if len(devices) == 0:
 59 |             raise Exception('no device detected, make sure the arm is powered, connected and you have read-write permissions on /dev/hidraw')
 60 | 
 61 |         self._device = devices[0]
 62 | 
 63 |         # open a device
 64 |         self._device.open()
 65 |         self._serial_number = self._device.serial_number.encode('ascii')
 66 |         print('connected to xArm controller serial number: ' + self._serial_number)
 67 | 
 68 |         # compute bits_to_rads: Lewansoul bits are 1000 for 240 degree
 69 |         # range, then convert from degrees to radians
 70 |         self.bits_to_si = math.radians((240.0 / 1000.0))
 71 | 
 72 |         # list of arms
 73 |         self._arms = []
 74 | 
 75 |         # lock to make sure read/writes are safe
 76 |         self._lock = threading.RLock()
 77 | 
 78 | 
 79 |     def __del__(self):
 80 |         print('closing xArm controller')
 81 |         self.shutdown()
 82 |         self._device.close()
 83 | 
 84 | 
 85 |     def add_arm(self, name, config_file, urdf_file = ''):
 86 |         new_arm = lewansoul_xarm.arm(self, name, config_file, urdf_file)
 87 |         self._arms.append(new_arm)
 88 |         return new_arm
 89 | 
 90 | 
 91 |     def measured_jp(self, servo_ids):
 92 |         """
 93 |         Read the position of all servos
 94 |         ServoPositionRead 21 (byte)count { (byte)id }; (byte)count { (byte)id (ushort)position }
 95 |         """
 96 |         nb_servos = len(servo_ids)
 97 |         with self._lock:
 98 |             self._device.write(itertools.chain([0x55, 0x55,
 99 |                                                 3 + nb_servos, # length of command
100 |                                                 CMD_MULT_SERVO_POS_READ,
101 |                                                 nb_servos]
102 |                                                ,
103 |                                                servo_ids))
104 |             response = self._device.read()
105 | 
106 |         # check first two elements of answer
107 |         assert response[0] == 0x55
108 |         assert response[1] == 0x55
109 |         # response should march number of servos
110 |         count = response[4]
111 |         assert count == nb_servos
112 |         # array used to store SI positions
113 |         positions = numpy.zeros(nb_servos)
114 |         for i in range(nb_servos):
115 |             # check that servo ids match
116 |             id = response[5 + 3 * i]
117 |             assert id == servo_ids[i]
118 |             # compute joint position
119 |             p_lsb = response[5 + 3 * i + 1] # least significant bit
120 |             p_msb = response[5 + 3 * i + 2] # most significant bit
121 |             pos_bits = (p_msb << 8) + p_lsb
122 |             positions[i] = pos_bits * self.bits_to_si
123 |         return positions
124 | 
125 | 
126 |     def disable(self, servo_ids):
127 |         # command
128 |         nb_servos = len(servo_ids)
129 |         command = [0x55, 0x55,
130 |                    nb_servos + 3, # msg length
131 |                    CMD_MULT_SERVO_UNLOAD,
132 |                    nb_servos]
133 |         with self._lock:
134 |             self._device.write(itertools.chain(command, servo_ids))
135 | 
136 | 
137 |     def shutdown(self):
138 |         for arm in self._arms:
139 |             print('shutting down ' + arm._name)
140 |             self.disable(arm._servo_ids)
141 | 
142 | 
143 |     def move_jp(self, servo_ids, goals_si, time_s = 0.0):
144 |         nb_servos = len(goals_si)
145 |         assert nb_servos == len(servo_ids)
146 | 
147 |         # should be replaced by computation using optimal joint velocities
148 |         if time_s == 0.0:
149 |             time_s = 2.0
150 |         elif time_s > 20:
151 |             time_s = 20.0
152 |         time_ms = int(time_s * 1000)
153 | 
154 |         # convert
155 |         goals = goals_si / self.bits_to_si
156 | 
157 |         # command
158 |         command = [0x55, 0x55,
159 |                    5 + nb_servos * 3, # msg length
160 |                    CMD_SERVO_MOVE,
161 |                    nb_servos]
162 |         command.extend(integer_to_bits(time_ms))
163 |         for i, servo_id in enumerate(servo_ids):
164 |             command.append(servo_id)
165 |             command.extend(integer_to_bits(int(goals[i])))
166 | 
167 |         with self._lock:
168 |             self._device.write(command)
169 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
  1 | ## Introduction
  2 | 
  3 | This package allows to control a Lewansoul xArm robot from a Linux PC with ROS.  It uses the HID interface between your PC and the Arduino board imbedded in the xArm.  Said Arduino board then communicates with the servos (6 of them, LX 15 D) using a serial TTL UART connection.  It is also possible to communicate directly with the servo units with a serial communication (and bypass the arduino board on the base) but this is not supported by the code in this repository.
  4 | 
  5 | To note, the arm is sometimes listed as a 6-DOF robot (degree of freedom) but it is actually a 5 DOF + 1 for the gripper.  So it is an under actuated arm and one can't reach all possible 3D positions/orientations.
  6 | 
  7 | If you find bugs or want to add some extra features, don't hesitate to create a pull request :-)
  8 | 
  9 | Naming convention is loosely based on CRTK: https://github.com/collaborative-robotics/documentation/wiki/Robot-API-motion
 10 | 
 11 | ## Requirements
 12 | 
 13 | ### Software
 14 | 
 15 | Code tested on Linux Ubuntu 18.04 using Python 2.  The following packages are required:
 16 | ```sh
 17 | sudo apt install libhidapi-hidraw0 libhidapi-libusb0 python-hid
 18 | pip install --user easyhid
 19 | ```
 20 | 
 21 | ### Permissions
 22 | 
 23 | The HID device is created with the permissions `root.root`, `0600` by default which means that users can communicate with the robot unless they escalate to root privileges (`sudo`).
 24 | 
 25 | To avoid this you should create a `udev` rule.  To get the proper attributes, you must first identify the `hidraw` device created for your device.  You can use `dmesg -w` in a shell and then plug/power on the arm.  Let's assume it's `hidraw3` and launch `sudo udevadm info --attribute-walk /dev/hidraw3 | less`.
 26 | 
 27 | You can create a rule using the product name since it seems specific enough to identify the xArm Arduino board.  Create the file
 28 | `/etc/udev/rules.d/80-usb-xarm.rules` with the content `SUBSYSTEM=="hidraw", ATTRS{product}=="LOBOT", GROUP="dialout", MODE="0660"` (note that all users will need to be members of the group `dialout`).  To apply the rules without rebooting: `sudo udevadm control --reload-rules` and `sudo udevadm trigger` (this is a one time setup).
 29 | 
 30 | ### `hidraw` device not created properly
 31 | 
 32 | The last issue is a lot more annoying and we don't have a good fix.  From time to time, the `hidraw` device is not properly created.  I found an error message in ` /var/log/syslog` related to FuEngine.  This seems to be a feature to automatically check/upgrade a device's firmware.  The only way we found to avoid this is to disable the service `fwupd` using `sudo service fwupd stop` (has to be done after every reboot) or ` sudo apt remove fwupd ` (completely remove fwupd).  If anyone has a better solution, please raise your hand.
 33 | 
 34 | ## Build
 35 | 
 36 | ```sh
 37 | cd ~/catkin_ws/src
 38 | catkin build
 39 | ```
 40 | 
 41 | ## Usage
 42 | 
 43 | This is for the low-level controller, i.e. not using ROS.  This code allows to create an "arm" using any subset of servos (in the example, I randomly used 5, 4 and 3).  In practice, this allows to create a first group of 5 servos (the kinematic part of the arm) and a second group with one servo (the gripper).   The ROS example below handles the grouping and also includes URDF import for the kinematic part.
 44 | 
 45 | ```python
 46 | import math
 47 | import lewansoul_xarm
 48 | c = lewansoul_xarm.controller()
 49 | HIDDevice:
 50 |     /dev/hidraw3 | 483:5750 | MyUSB_HID | LOBOT | 49770F673737
 51 |     release_number: 513
 52 |     usage_page: 0
 53 |     usage: 0
 54 |     interface_number: 0
 55 | connected to xArm controller serial number: 49770F673737
 56 | a = c.add_arm(5, 4, 3)
 57 | a.disable() # turn off servos
 58 | # now manually move the arm to zero position
 59 | a.calibrate() # this is a one time thing, you can re-use the calibration later on
 60 | a.home()
 61 | a.is_homed()
 62 | p = a.measured_jp()
 63 | p[0] = p[0] + math.pi / 4.0 # start with a small motion
 64 | a.move_jp(p)
 65 | ```
 66 | 
 67 | 
 68 | ## ROS notes
 69 | 
 70 | ```sh
 71 | sudo apt install ros-melodic-joint-state-publisher-gui
 72 | ```
 73 | 
 74 | To start the controller ROS node:
 75 | ```sh
 76 | rosrun lewansoul_xarm xArm-ros.py
 77 | ```
 78 | 
 79 | The ROS node is defined in `scripts/xArm-ros.py`.  It treats the first 5 servos as a kinematic chain and the last servo is used for the gripper.
 80 | 
 81 | To calibrate (this has to be done for `/xArm` and `/gripper`):
 82 | ```sh
 83 | # disable servos so user can move the arm manually
 84 | rostopic pub -1 /xArm/disable std_msgs/Empty "{}"
 85 | # now move the arm to mechanical zero position...
 86 | # once done, initiate calibration.   This will save the joint offsets
 87 | rostopic pub -1 /xArm/calibrate std_msgs/Empty "{}"
 88 | # and now re-enable the arm.  It is now ready to use...
 89 | rostopic pub -1 /xArm/enable std_msgs/Empty "{}"
 90 | ```
 91 | 
 92 | Calibration results are saved in files and can be reloaded automatically.  The calibration files should be uniquely named using the controller's serial number and arm name.
 93 | 
 94 | For RViz:
 95 | ```sh
 96 | roslaunch lewansoul_xarm display.launch
 97 | ```
 98 | 
 99 | # Links
100 | 
101 | ## Hardware
102 | * Manufacturer: Hiwonder in HK - I’m not sure what is the proper brand for this product, maybe it is sold under two different names, either Lewansoul or Hiwonder: https://www.hiwonder.hk 
103 | * Amazon page: https://smile.amazon.com/gp/product/B0793PFGCY
104 | * YouTube Channel with assembly instructions:
105 |   * https://www.youtube.com/watch?v=0nGwqJCcUuE
106 |   * https://www.youtube.com/watch?v=IjKZ74H3Q4o&t=125s
107 | * Vendor for single actuator, has more technical data: https://usa.banggood.com/LOBOT-LX-15D-Serial-Bus-Servo-Metal-Gear-Dual-Shaft-Large-Torque-For-RC-Robot-p-1372532.html
108 | * Serial communication protocol for LX 16 A (not the exact model used in our arm): https://github.com/RoboticsBrno/Robotic-arm/blob/master/Hardware/Servo-LX16-A/Lewansoul%20Bus%20Servo%20Communication%20Protocol.pdf
109 | * LX 16 A documentation (not the exact model used in our arm): https://images-na.ssl-images-amazon.com/images/I/91DLnW8nTnL.pdf
110 | * CAD for LX-15D, the servo used in the arm we have: https://grabcad.com/library/lewansoul-lx-15d-servo-1
111 | 
112 | ## Software
113 | * Light Python wrapper for direct servo: https://github.com/Roger-random/SGVHAK_Rover/blob/master/SGVHAK_Rover/lewansoul_wrapper.py
114 | * Another Python wrapper with GUI for direct servo: https://github.com/maximkulkin/lewansoul-lx16a
115 | * Python over HID (code I started from):
116 |   * https://gist.github.com/maximecb/7fd42439e8a28b9a74a4f7db68281071
117 |   * https://github.com/ccourson/LewanSoul-xArm/issues/7?fbclid=IwAR0rXDFUnZRi0rPvOVP7_1yvddLER_ChwoFYI8bTqBlQylNzd1iQYaT3XX4#issuecomment-504570455
118 | 
119 | 
120 | 
121 | 
122 | 


--------------------------------------------------------------------------------
/rviz/urdf.rviz:
--------------------------------------------------------------------------------
  1 | Panels:
  2 |   - Class: rviz/Displays
  3 |     Help Height: 78
  4 |     Name: Displays
  5 |     Property Tree Widget:
  6 |       Expanded:
  7 |         - /Global Options1
  8 |         - /Status1
  9 |         - /Grid1
 10 |         - /RobotModel1
 11 |         - /TF1
 12 |       Splitter Ratio: 0.5
 13 |     Tree Height: 418
 14 |   - Class: rviz/Selection
 15 |     Name: Selection
 16 |   - Class: rviz/Tool Properties
 17 |     Expanded:
 18 |       - /2D Pose Estimate1
 19 |       - /2D Nav Goal1
 20 |       - /Publish Point1
 21 |     Name: Tool Properties
 22 |     Splitter Ratio: 0.5886790156364441
 23 |   - Class: rviz/Views
 24 |     Expanded:
 25 |       - /Current View1
 26 |     Name: Views
 27 |     Splitter Ratio: 0.5
 28 |   - Class: rviz/Time
 29 |     Experimental: false
 30 |     Name: Time
 31 |     SyncMode: 0
 32 |     SyncSource: ""
 33 | Preferences:
 34 |   PromptSaveOnExit: true
 35 | Toolbars:
 36 |   toolButtonStyle: 2
 37 | Visualization Manager:
 38 |   Class: ""
 39 |   Displays:
 40 |     - Alpha: 0.5
 41 |       Cell Size: 1
 42 |       Class: rviz/Grid
 43 |       Color: 160; 160; 164
 44 |       Enabled: true
 45 |       Line Style:
 46 |         Line Width: 0.029999999329447746
 47 |         Value: Lines
 48 |       Name: Grid
 49 |       Normal Cell Count: 0
 50 |       Offset:
 51 |         X: 0
 52 |         Y: 0
 53 |         Z: 0
 54 |       Plane: XY
 55 |       Plane Cell Count: 10
 56 |       Reference Frame: <Fixed Frame>
 57 |       Value: true
 58 |     - Alpha: 1
 59 |       Class: rviz/RobotModel
 60 |       Collision Enabled: false
 61 |       Enabled: true
 62 |       Links:
 63 |         All Links Enabled: true
 64 |         Expand Joint Details: false
 65 |         Expand Link Details: false
 66 |         Expand Tree: false
 67 |         Link Tree Style: Links in Alphabetic Order
 68 |         base_link:
 69 |           Alpha: 1
 70 |           Show Axes: false
 71 |           Show Trail: false
 72 |         link1:
 73 |           Alpha: 1
 74 |           Show Axes: false
 75 |           Show Trail: false
 76 |           Value: true
 77 |         link2:
 78 |           Alpha: 1
 79 |           Show Axes: false
 80 |           Show Trail: false
 81 |           Value: true
 82 |         link3:
 83 |           Alpha: 1
 84 |           Show Axes: false
 85 |           Show Trail: false
 86 |           Value: true
 87 |         link4:
 88 |           Alpha: 1
 89 |           Show Axes: false
 90 |           Show Trail: false
 91 |         link5:
 92 |           Alpha: 1
 93 |           Show Axes: false
 94 |           Show Trail: false
 95 |         world:
 96 |           Alpha: 1
 97 |           Show Axes: false
 98 |           Show Trail: false
 99 |       Name: RobotModel
100 |       Robot Description: robot_description
101 |       TF Prefix: ""
102 |       Update Interval: 0
103 |       Value: true
104 |       Visual Enabled: true
105 |     - Class: rviz/TF
106 |       Enabled: true
107 |       Frame Timeout: 15
108 |       Frames:
109 |         All Enabled: true
110 |         base_link:
111 |           Value: true
112 |         link1:
113 |           Value: true
114 |         link2:
115 |           Value: true
116 |         link3:
117 |           Value: true
118 |         link4:
119 |           Value: true
120 |         link5:
121 |           Value: true
122 |         world:
123 |           Value: true
124 |       Marker Scale: 0.5
125 |       Name: TF
126 |       Show Arrows: false
127 |       Show Axes: false
128 |       Show Names: false
129 |       Tree:
130 |         world:
131 |           base_link:
132 |             link1:
133 |               link2:
134 |                 link3:
135 |                   link4:
136 |                     link5:
137 |                       {}
138 |       Update Interval: 0
139 |       Value: true
140 |   Enabled: true
141 |   Global Options:
142 |     Background Color: 48; 48; 48
143 |     Default Light: true
144 |     Fixed Frame: base_link
145 |     Frame Rate: 30
146 |   Name: root
147 |   Tools:
148 |     - Class: rviz/Interact
149 |       Hide Inactive Objects: true
150 |     - Class: rviz/MoveCamera
151 |     - Class: rviz/Select
152 |     - Class: rviz/FocusCamera
153 |     - Class: rviz/Measure
154 |     - Class: rviz/SetInitialPose
155 |       Theta std deviation: 0.2617993950843811
156 |       Topic: /initialpose
157 |       X std deviation: 0.5
158 |       Y std deviation: 0.5
159 |     - Class: rviz/SetGoal
160 |       Topic: /move_base_simple/goal
161 |     - Class: rviz/PublishPoint
162 |       Single click: true
163 |       Topic: /clicked_point
164 |   Value: true
165 |   Views:
166 |     Current:
167 |       Class: rviz/Orbit
168 |       Distance: 0.8512046337127686
169 |       Enable Stereo Rendering:
170 |         Stereo Eye Separation: 0.05999999865889549
171 |         Stereo Focal Distance: 1
172 |         Swap Stereo Eyes: false
173 |         Value: false
174 |       Focal Point:
175 |         X: 0.054874010384082794
176 |         Y: -0.037138987332582474
177 |         Z: 0.08454325050115585
178 |       Focal Shape Fixed Size: false
179 |       Focal Shape Size: 0.05000000074505806
180 |       Invert Z Axis: false
181 |       Name: Current View
182 |       Near Clip Distance: 0.009999999776482582
183 |       Pitch: 0.6003977060317993
184 |       Target Frame: <Fixed Frame>
185 |       Value: Orbit (rviz)
186 |       Yaw: 2.3303987979888916
187 |     Saved: ~
188 | Window Geometry:
189 |   Displays:
190 |     collapsed: false
191 |   Height: 882
192 |   Hide Left Dock: false
193 |   Hide Right Dock: false
194 |   QMainWindow State: 000000ff00000000fd000000040000000000000156000002cefc0200000008fb0000001200530065006c0065006300740069006f006e010000003d0000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000de0000022d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002cefc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002ce000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005df00000044fc0100000002fb0000000800540069006d00650100000000000005df000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000036e000002ce00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
195 |   Selection:
196 |     collapsed: false
197 |   Time:
198 |     collapsed: false
199 |   Tool Properties:
200 |     collapsed: false
201 |   Views:
202 |     collapsed: false
203 |   Width: 1503
204 |   X: 347
205 |   Y: 22
206 | 


--------------------------------------------------------------------------------
/urdf/xArm.urdf:
--------------------------------------------------------------------------------
  1 | <?xml version="1.0" ?>
  2 | <robot name="xArm">
  3 | 
  4 |   <link name="world"/>
  5 | 
  6 |   <joint name="world_joint" type="fixed">
  7 |     <parent link="world"/>
  8 |     <child link="base_link"/>
  9 |     <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.05"/>
 10 |   </joint>
 11 | 
 12 |   <link name="base_link"/>
 13 | 
 14 |   <joint name="arm1" type="continuous">
 15 |     <parent link="base_link"/>
 16 |     <child link="link1"/>
 17 |     <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.05"/>
 18 |     <axis xyz="0 0 1"/>
 19 |   </joint>
 20 | 
 21 |   <link name="link1">
 22 |     <visual>
 23 |       <origin rpy="0 0 0.0" xyz="0 0 0.0065"/>
 24 |       <geometry>
 25 |         <cylinder length="0.013" radius="0.04"/>
 26 |       </geometry>
 27 |       <material name="blue">
 28 | 	<color rgba="0.3 0.5 0.9 1.0"/>
 29 |       </material>
 30 |     </visual>
 31 |     <visual>
 32 |       <origin rpy="0 0 0" xyz="0.011 0.0 0.01625"/>
 33 |       <geometry>
 34 |         <box size="0.044 0.0285 0.0065"/>
 35 |       </geometry>
 36 |       <material name="blue">
 37 | 	<color rgba="0.3 0.5 0.9 1.0"/>
 38 |       </material>
 39 |     </visual>
 40 |     <visual>
 41 |       <origin rpy="0 0 0" xyz="0.011 0.0 0.031"/>
 42 |       <geometry>
 43 |         <box size="0.044 0.0285 0.023"/>
 44 |       </geometry>
 45 |       <material name="black">
 46 | 	<color rgba="0.2 0.2 0.2 1"/>
 47 |       </material>
 48 |     </visual>
 49 |   </link>
 50 | 
 51 |   <joint name="arm2" type="continuous">
 52 |     <parent link="link1"/>
 53 |     <child link="link2"/>
 54 |     <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0425"/>
 55 |     <axis xyz="0 1 0"/>
 56 |   </joint>
 57 | 
 58 |   <link name="link2">
 59 |     <visual>
 60 |       <origin rpy="0 0 0" xyz="0.0 0.02 0.0135"/>
 61 |       <geometry>
 62 |         <box size="0.032 0.002 0.052"/>
 63 |       </geometry>
 64 |       <material name="blue">
 65 | 	<color rgba="0.3 0.5 0.9 1.0"/>
 66 |       </material>
 67 |     </visual>
 68 |     <visual>
 69 |       <origin rpy="0 0 0" xyz="0.0 -0.02 0.0135"/>
 70 |       <geometry>
 71 |         <box size="0.032 0.002 0.052"/>
 72 |       </geometry>
 73 |       <material name="blue">
 74 | 	<color rgba="0.3 0.5 0.9 1.0"/>
 75 |       </material>
 76 |     </visual>
 77 |     <visual>
 78 |       <origin rpy="0 0 0" xyz="0.0 0.0 0.0385"/>
 79 |       <geometry>
 80 |         <box size="0.032 0.042 0.002"/>
 81 |       </geometry>
 82 |       <material name="blue">
 83 | 	<color rgba="0.3 0.5 0.9 1.0"/>
 84 |       </material>
 85 |     </visual>
 86 |     <visual>
 87 |       <origin rpy="0 0 1.57075" xyz="0 0 0.054"/>
 88 |       <geometry>
 89 |         <cylinder length="0.029" radius="0.012"/>
 90 |       </geometry>
 91 |       <material name="white">
 92 | 	<color rgba="0.9 0.9 0.9 1.0"/>
 93 |       </material>
 94 |     </visual>
 95 |     <visual>
 96 |       <origin rpy="0 0 0" xyz="0.0 0.0 0.0695"/>
 97 |       <geometry>
 98 |         <box size="0.025 0.042 0.002"/>
 99 |       </geometry>
100 |       <material name="blue">
101 | 	<color rgba="0.3 0.5 0.9 1.0"/>
102 |       </material>
103 |     </visual>
104 |     <visual>
105 |       <origin rpy="0 0 0" xyz="0.0 0.02 0.091"/>
106 |       <geometry>
107 |         <box size="0.025 0.002 0.045"/>
108 |       </geometry>
109 |       <material name="blue">
110 | 	<color rgba="0.3 0.5 0.9 1.0"/>
111 |       </material>
112 |     </visual>
113 |     <visual>
114 |       <origin rpy="0 0 0" xyz="0.0 -0.02 0.091"/>
115 |       <geometry>
116 |         <box size="0.025 0.002 0.045"/>
117 |       </geometry>
118 |       <material name="blue">
119 | 	<color rgba="0.3 0.5 0.9 1.0"/>
120 |       </material>
121 |     </visual>
122 |   </link>
123 | 
124 |   <joint name="arm3" type="continuous">
125 |     <parent link="link2"/>
126 |     <child link="link3"/>
127 |     <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0965"/>
128 |     <axis xyz="0 1 0"/>
129 |   </joint>
130 | 
131 |   <link name="link3">
132 |     <visual>
133 |       <origin rpy="0 0 0" xyz="0.0 0.0 0.011"/>
134 |       <geometry>
135 |         <box size="0.023 0.0285 0.044"/>
136 |       </geometry>
137 |       <material name="black">
138 | 	<color rgba="0.2 0.2 0.2 1"/>
139 |       </material>
140 |     </visual>
141 |     <visual>
142 |       <origin rpy="0 0 0" xyz="0.0 0.0 0.035"/>
143 |       <geometry>
144 |         <box size="0.023 0.0285 0.004"/>
145 |       </geometry>
146 |       <material name="blue">
147 | 	<color rgba="0.3 0.5 0.9 1.0"/>
148 |       </material>
149 |     </visual>
150 |     <visual>
151 |       <origin rpy="0 0 1.57075" xyz="0 0 0.0475"/>
152 |       <geometry>
153 |         <cylinder length="0.021" radius="0.0105"/>
154 |       </geometry>
155 |       <material name="white">
156 | 	<color rgba="0.9 0.9 0.9 1.0"/>
157 |       </material>
158 |     </visual>
159 |     <visual>
160 |       <origin rpy="0 0 0" xyz="0.0 0.0 0.060"/>
161 |       <geometry>
162 |         <box size="0.023 0.0285 0.004"/>
163 |       </geometry>
164 |       <material name="blue">
165 | 	<color rgba="0.3 0.5 0.9 1.0"/>
166 |       </material>
167 |     </visual>
168 |     <visual>
169 |       <origin rpy="0 0 0" xyz="0.0 0.0 0.084"/>
170 |       <geometry>
171 |         <box size="0.023 0.0285 0.044"/>
172 |       </geometry>
173 |       <material name="black">
174 | 	<color rgba="0.2 0.2 0.2 1"/>
175 |       </material>
176 |     </visual>
177 |   </link>
178 | 
179 |   <joint name="arm4" type="continuous">
180 |     <parent link="link3"/>
181 |     <child link="link4"/>
182 |     <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0965"/>
183 |     <axis xyz="0 1 0"/>
184 |   </joint>
185 | 
186 |   <link name="link4">
187 |     <visual>
188 |       <origin rpy="0 0 0" xyz="0.0 0.02 0.0195"/>
189 |       <geometry>
190 |         <box size="0.025 0.002 0.064"/>
191 |       </geometry>
192 |       <material name="blue">
193 | 	<color rgba="0.3 0.5 0.9 1.0"/>
194 |       </material>
195 |     </visual>
196 |     <visual>
197 |       <origin rpy="0 0 0" xyz="0.0 -0.02 0.0195"/>
198 |       <geometry>
199 |         <box size="0.025 0.002 0.064"/>
200 |       </geometry>
201 |       <material name="blue">
202 | 	<color rgba="0.3 0.5 0.9 1.0"/>
203 |       </material>
204 |     </visual>
205 |     <visual>
206 |       <origin rpy="0 0 0" xyz="0.0 0.0 0.0505"/>
207 |       <geometry>
208 |         <box size="0.025 0.042 0.002"/>
209 |       </geometry>
210 |       <material name="blue">
211 | 	<color rgba="0.3 0.5 0.9 1.0"/>
212 |       </material>
213 |     </visual>
214 |     <visual>
215 |       <origin rpy="0 0 0" xyz="-0.011 0.0 0.03525"/>
216 |       <geometry>
217 |         <box size="0.044 0.023 0.0285"/>
218 |       </geometry>
219 |       <material name="black">
220 | 	<color rgba="0.2 0.2 0.2 1"/>
221 |       </material>
222 |     </visual>
223 |   </link>
224 | 
225 |   <joint name="arm5" type="continuous">
226 |     <parent link="link4"/>
227 |     <child link="link5"/>
228 |     <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.055"/>
229 |     <axis xyz="0 0 1"/>
230 |   </joint>
231 | 
232 |   <link name="link5">
233 |     <visual>
234 |       <origin rpy="0 0 0.0" xyz="0 0 0"/>
235 |       <geometry>
236 |         <cylinder length="0.004" radius="0.01"/>
237 |       </geometry>
238 |       <material name="black">
239 | 	<color rgba="0.2 0.2 0.2 1.0"/>
240 |       </material>
241 |     </visual>
242 |   </link>
243 | </robot>
244 | 


--------------------------------------------------------------------------------
/src/lewansoul_xarm/arm.py:
--------------------------------------------------------------------------------
  1 | #  Author(s):  Anton Deguet
  2 | #  Created on: 2020-04
  3 | 
  4 | # (C) Copyright 2020 Johns Hopkins University (JHU), All Rights Reserved.
  5 | 
  6 | # --- begin cisst license - do not edit ---
  7 | 
  8 | # This software is provided "as is" under an open source license, with
  9 | # no warranty.  The complete license can be found in license.txt and
 10 | # http://www.cisst.org/cisst/license.txt.
 11 | 
 12 | # --- end cisst license ---
 13 | 
 14 | """
 15 | 
 16 | """
 17 | 
 18 | import numpy
 19 | import json
 20 | import urdf_parser_py.urdf
 21 | import kdl_parser_py.urdf
 22 | import PyKDL
 23 | 
 24 | class arm(object):
 25 |     """Arm class to define a logical arm on top of the servo controller.
 26 |     This allows to use a subset of servos to define different kinematic
 27 |     chains.
 28 | 
 29 |     The most practical use is to separate the servos used for the arm
 30 |     kinematic from the gripper itself.  The xArm is actually not a 6
 31 |     DOFs robot, it has 5 servos ([6, 5, 4, 3, 2]) for the arm and the
 32 |     last one ([1]).  As such it is underactuated.
 33 | 
 34 |     The typical use is:
 35 |     import lewansoul_xarm
 36 |     c = lewansoul_xarm.controller()
 37 |     arm = c.add_arm(6, 5, 4, 3, 2)
 38 |     gripper = c.add_arm(1)
 39 |     arm.home()
 40 |     position = arm.measured_jp()
 41 |     position[0] = position[0] + math.pi / 4.0
 42 |     arm.move_jp(position)
 43 |     """
 44 | 
 45 |     def __init__(self, controller, name, config_file, urdf_file = ''):
 46 |         """Initialize the robot using a controller and a list of servo ids.
 47 |         The constructor also creates a unique id for the arm using the
 48 |         controller's serial number and the list of servo ids used for
 49 |         the arm.
 50 |         """
 51 |         self._controller = controller
 52 |         self._name = name
 53 |         self._config_file = config_file
 54 |         self._configuration = {}
 55 |         self._servo_ids = []
 56 |         self._is_homed = False
 57 |         self._has_kin = False
 58 |         self.configure(config_file)
 59 |         if not urdf_file == '':
 60 |             self._has_kin = True
 61 |             self.configure_urdf(urdf_file)
 62 | 
 63 | 
 64 |     def __del__(self):
 65 |         self.disable()
 66 | 
 67 | 
 68 |     def is_homed(self):
 69 |         return self._is_homed
 70 | 
 71 | 
 72 |     def calibrate(self, reference_position_si = 0.0):
 73 |         """Calibrate the servos position offsets.  The xArm servos are
 74 |         absolute, i.e. you can turn off/on the controller and the
 75 |         origin will not be reset.  The issue is that "zero" can be
 76 |         anywhere depending on how you assembled he arm.  The calibrate
 77 |         method computes and offset between the reference positions
 78 |         ([0..0] by default) and the current position.
 79 | 
 80 |         The calibration procedure is:
 81 |         - disable the arm so the servos are free to move (arm.disable())
 82 |         - position the arm in "zero" position
 83 |         - call arm.calibrate()
 84 | 
 85 |         The position offsets are used after the calibration and saved
 86 |         in a JSON configuration file using the arm unique id (see
 87 |         __init__ documentation).  Future calls of home() or
 88 |         configure() will try to locate the JSON configuration file and
 89 |         load it.  This allows to perform the calibration to be a one
 90 |         time task.
 91 |         """
 92 |         # get raw positions from controller
 93 |         position_si = numpy.multiply(self._directions, self._controller.measured_jp(self._servo_ids))
 94 |         self._position_offsets = reference_position_si - position_si
 95 |         # save calibrations results for later use, convert array to list
 96 |         self._configuration['offsets'] = self._position_offsets.tolist()
 97 |         with open(self._config_file, 'w') as f:
 98 |             f.write(json.dumps(self._configuration, sort_keys = True,
 99 |                                indent = 4, separators = (', ', ': ')))
100 | 
101 |         # no need to home now
102 |         self._is_homed = True
103 | 
104 | 
105 |     def configure(self, config_file):
106 |         """Configure the arm using a JSON configuration file.  The
107 |         configuration file must contain the board ids, controller
108 |         serial number and the position offsets vector (see
109 |         calibrate()).
110 |         """
111 |         try:
112 |             with open(config_file, 'r') as f:
113 |                 self._configuration = json.load(f)
114 |         except ValueError as e:
115 |             print(e)
116 |             raise
117 |         except:
118 |             raise
119 | 
120 |         # serial number [required]
121 |         if not 'controller-serial-number' in self._configuration.keys():
122 |             raise Exception('controller-serial-number is not defined in ' + config_file)
123 |         assert self._configuration['controller-serial-number'] == self._controller._serial_number
124 | 
125 |         # servo ids [required]
126 |         if not 'servo-ids' in self._configuration.keys():
127 |             raise Exception('servo-ids are not defined in ' + config_file)
128 |         self._servo_ids = self._configuration['servo-ids']
129 | 
130 |         # offsets [optional]
131 |         if not 'offsets' in self._configuration.keys():
132 |             self._position_offsets = numpy.zeros(len(self._servo_ids))
133 |         else:
134 |             if len(self._configuration['offsets']) != len(self._servo_ids):
135 |                 raise Exception('incorrect number of offsets found in ' + config_file)
136 |             else:
137 |                 self._position_offsets = numpy.array(self._configuration['offsets'])
138 | 
139 |         # directions [optional]
140 |         if not 'directions' in self._configuration.keys():
141 |             self._directions = numpy.ones(len(self._servo_ids))
142 |         else:
143 |             if len(self._configuration['directions']) != len(self._servo_ids):
144 |                 raise Exception('incorrect number of directions found in ' + config_file)
145 |             else:
146 |                 self._directions = numpy.array(self._configuration['directions'])
147 | 
148 |         self._measured_jp = numpy.zeros(len(self._servo_ids))
149 |         self._goal_jp =  numpy.zeros(len(self._servo_ids))
150 |         self._measured_cp = PyKDL.Frame()
151 | 
152 | 
153 |     def configure_urdf(self, urdf_file):
154 |         self._kdl_urdf = urdf_parser_py.urdf.URDF.from_xml_file(urdf_file)
155 |         status, self._kdl_tree = kdl_parser_py.urdf.treeFromUrdfModel(self._kdl_urdf)
156 |         assert status
157 |         self._kdl_chain = self._kdl_tree.getChain('base_link', 'link5')
158 |         self._kdl_fk = PyKDL.ChainFkSolverPos_recursive(self._kdl_chain)
159 |         self._kdl_joints = PyKDL.JntArray(5)
160 | 
161 | 
162 | 
163 |     def enable(self):
164 |         """Enable the servo controllers.  This method first read the current
165 |         positions and then asks the servos to "go to" the current
166 |         position.
167 |         """
168 |         # afaik there's no command to just enable the arm
169 |         position = self.measured_jp()
170 |         self.move_jp(position)
171 | 
172 | 
173 |     def disable(self):
174 |         """Disable the servo controllers
175 |         """
176 |         self._controller.disable(self._servo_ids)
177 | 
178 | 
179 |     def home(self):
180 |         if not self._is_homed:
181 |             self.get_data()
182 |             self.move_jp(self._measured_jp)
183 |             self._is_homed = True
184 | 
185 | 
186 |     def get_data(self):
187 |         self._measured_jp = numpy.multiply(self._directions, self._controller.measured_jp(self._servo_ids)) + self._position_offsets
188 |         if self._has_kin:
189 |             for i in range(5):
190 |                 self._kdl_joints[i] = self._measured_jp[i]
191 |             status = self._kdl_fk.JntToCart(self._kdl_joints, self._measured_cp, 5)
192 | 
193 | 
194 |     def measured_jp(self):
195 |         return self._measured_jp
196 | 
197 | 
198 |     def measured_cp(self):
199 |         return self._measured_cp
200 | 
201 | 
202 |     def goal_jp(self):
203 |         """Return the last joint position goal.  The goal is based on the
204 |         last move_jp call.
205 |         """
206 |         return self._goal_jp
207 | 
208 | 
209 |     def move_jp(self, goals_si, time_s = 0.0):
210 |         self._goal_jp = goals_si[:]
211 |         self._controller.move_jp(self._servo_ids,
212 |                                  numpy.multiply(self._directions, (goals_si - self._position_offsets)),
213 |                                  time_s)
214 | 


--------------------------------------------------------------------------------