|
| 1 | +# Hardware Testing - ArmMotus EMU (aka M3) 3D Manipulandum |
| 2 | + |
| 3 | +This pages introduces the M3DemoMachine, an example CORC app showing the basic use of the M3 3D manipulandum. |
| 4 | + |
| 5 | +The M3 is a 3 DoFs impedance based robot developed by Fourier Intelligence: |
| 6 | + |
| 7 | + |
| 8 | +ArmMotus EMU (aka M3) and reference coordinates. |
| 9 | + |
| 10 | +The state machine code can be found in the folder `src/apps/M3DemoMachine`. |
| 11 | + |
| 12 | +It demonstrates the use of: |
| 13 | +- The different control modes of M3 (position, velocity, or impedance) |
| 14 | +- The use of the different kinematic models of the robot and associated model parameters (loaded from a YAML file) |
| 15 | +- The use of a standard joystick as a control input |
| 16 | +- The use of the libFLNL comunication library to pusblish the robot state in a Unity software and send commands to the state machine |
| 17 | + |
| 18 | + |
| 19 | +## Running the state machine |
| 20 | + |
| 21 | +In the CMakeLists.txt select the M3DemoMachine and set the flags for using a real robot without ROS support: |
| 22 | + |
| 23 | +```cmake |
| 24 | +#set (STATE_MACHINE_NAME "ExoTestMachine") |
| 25 | +#set (STATE_MACHINE_NAME "M1DemoMachine") |
| 26 | +#set (STATE_MACHINE_NAME "M2DemoMachine") |
| 27 | +set (STATE_MACHINE_NAME "M3DemoMachine") |
| 28 | +#set (STATE_MACHINE_NAME "X2DemoMachine") |
| 29 | +#set (STATE_MACHINE_NAME "LoggingDevice") |
| 30 | +
|
| 31 | +# Comment to use actual hardware, uncomment for a nor robot (virtual) app |
| 32 | +set(NO_ROBOT OFF) |
| 33 | +
|
| 34 | +# ROS Flag. set ON if you want to use ROS. Else, set OFF. |
| 35 | +set(USE_ROS OFF) |
| 36 | +``` |
| 37 | + |
| 38 | +If you intend to cross-compile for a BeagleBone (Black or AI), run: `$ rm -r build && mkdir build && cd build && cmake -DCMAKE_TOOLCHAIN_FILE=../armhf.cmake ..` |
| 39 | + |
| 40 | +otherwise, to run the state machine locally use: `$ rm -r build && mkdir build && cd build && cmake .. ` |
| 41 | + |
| 42 | +Then simply compile the state machine: `$ make` |
| 43 | + |
| 44 | +This should create the application `M3DemoMachine` within the build folder. After initialising the CANbus (using the `initCAN0.sh` or `initCAN1.sh` script) you should be able to run the application, either locally or on the BB (through SSH). |
| 45 | + |
| 46 | +**WARNING:** With an M3 connected on the CAN bus the robot will imediatly start to move after running the application (to go in a calibration pose), ensure the space is clear around the robot. |
| 47 | + |
| 48 | +Once the calibration state is finished, you can circle through the different demo states using the keyboard (key 1) or using the joystick first button. |
| 49 | + |
| 50 | + |
| 51 | +## RobotM3 structure and interface |
| 52 | + |
| 53 | +### Robot model and parameters (YAML) |
| 54 | + |
| 55 | +The M3DemoMachine relies on primarily on the RobotM3 class which represents an M3 robot and associated models. Most of the kinematic and some dynamic parameters of the robot are defined within a YAML configuration file and follow the following model: |
| 56 | + |
| 57 | +<!-- language: lang-none --> |
| 58 | + /\ |
| 59 | + /- \ |
| 60 | + /- \ |
| 61 | + /- \ |
| 62 | + (L4) /- \ |
| 63 | + /- \ \ |
| 64 | + /- \ \ |
| 65 | + / \ \ (L2) |
| 66 | + /- \ \ |
| 67 | + M3/- \ M1 \ |
| 68 | + /- \ \ |
| 69 | + /- \ \ |
| 70 | + /- M2 \ \ |
| 71 | + /- \ \ |
| 72 | + +------+ \ \ |
| 73 | + | MTool| \ \ |
| 74 | + +------+ \ q1- (L0) |
| 75 | + \ ------- |
| 76 | + \ (L1) q0 |
| 77 | + q2 | |
| 78 | + | |
| 79 | + | |
| 80 | + | |
| 81 | + |
| 82 | + |
| 83 | +Extract from the YAML configuration file (`config/M3_params.yaml`) used to specify the robot parameters: |
| 84 | +```yaml |
| 85 | +EMU_FOURIER: |
| 86 | + dqMax: 360 # Max joint speed (deg.s-1) Set to 1 rot/s |
| 87 | + tauMax: 42 # Max joint torque (Nm) Set to max motor torque w/ 1:22 reduction (yes, this is the actual value!) |
| 88 | + qSigns: [-1, 1, -1] # Joints direction |
| 89 | + |
| 90 | + linkLengths: [0.056, 0.135, 0.5, 0.615] # Link lengths used for kinematic models (in m), excluding tool |
| 91 | + linkMasses: [0, 0.450, 0.400, 0.100, .270] # Link masses used for gravity compensation (in kg), excluding tool |
| 92 | + |
| 93 | + qLimits: [-43, 43, -15, 70, 0, 95] # Joints limits (in deg) {q1_min, q1_max, q2_min, q2_max, q3_min, q3_max} |
| 94 | + qCalibration: [+44, 76, 90] # Calibration configuration (in deg): posture in which the robot is when using the calibration procedure |
| 95 | + |
| 96 | + frictionVis: [0.2, 0.2, 0.2] # Joint viscous friction compensation coefficients |
| 97 | + frictionCoul: [0., 0., 0.] # Joint Coulomb (static) friction compensation coefficients |
| 98 | + |
| 99 | + #End-effector description |
| 100 | + tool: |
| 101 | + name: "Fourier Handle" |
| 102 | + length: 0.0 #Tool length from attachment |
| 103 | + mass: 0.345 #Tool mass in kg |
| 104 | +``` |
| 105 | +
|
| 106 | +The YAML configuration file to use and the corresponding robot name (model) can be selected from the RobotM3 constructor parameters: `RobotM3(std::string robot_name="", std::string yaml_config_file="")`. |
| 107 | + |
| 108 | +### Control methods |
| 109 | + |
| 110 | +The CORC M3 robot model has the following specific methods of interaction: |
| 111 | +- Obtaining current **joint state** (as for any CORC robot): `robot->getPosition()`, `robot->getVelocity()`, `robot->getTorque()`. |
| 112 | +- **Joint level interaction**: `setJointPosition(VM3 q)`, `setJointVelocity(VM3 q)` and `setJointTorque(VM3 tau)` allow to apply a position, velocity or torque control using an Eigen:vector of length 3. An example of the torque control can be found in the `M3CalibState` state. Note the use of `robot->initTorqueControl();` in the `entryCode()` method before applying torque control. |
| 113 | +- Obtaining current **end-effector state**: `robot->getEndEffPosition()`, `robot->getEndEffVelocity()`, `robot->getEndEffForce()`. Methods are also provided to obtain the filtered velocity and acceleration (obtained through differentiation and low-pass filtering). Additionally the pure interaction force at the end-effector, calculated using the robot model and motor torques from wich gravity compensation torques are subtracted can be obtained using the `robot->getInteractionForce()` method. |
| 114 | +- **End-effector space control** is available using: `setEndEffPosition(VM3 X)`, `setEndEffVelocity(VM3 dX)`, `setEndEffForce(VM3 F)`. These methods rely on the `inverseKinematic()` and robot Jacobian `J()` and assumes that the kinematic parameters loaded from the YAML file are correct and that the robot has been calibrated (see `applyCalibration()`). As for their joints counterparts, they require the proper use of the corresponding initTorque/Velocity/Position method beforehand. The command vectors are expressed in the robot base frame as shown on the picture above. An example of the use of the end-effector velocity control is available in the `M3EndEffDemo` state. |
| 115 | +- Finally, the method `setEndEffForceWithCompensation(VM3 F, bool friction_comp=true)` can be used to apply an end-effector force in addition to the **robot gravity self-compensation and friction compensation (optional)**. This method relies on the robot model and parameters (masses and friction coefficients) set in the YAML configuration file. The friction compensation uses a Coulomb + viscous friction model. |
| 116 | + |
| 117 | +See the Doxygen page of the `RobotM3` class for a full list of available methods. |
| 118 | + |
| 119 | +## Joystick |
| 120 | + |
| 121 | +The `RobotM3` class includes a Joystick input by default. The joystick, if connected, can be used within the different state machine states and transitions. |
| 122 | + |
| 123 | +An example of the use of the first stick of a joystick used as an input can be found in the `M3EndEffDemo` state. The method `robot->joystick->getAxis(i)` returns a value proportional to the position of the stick direction `i`, which is used as a velocity command. |
| 124 | + |
| 125 | +Additionally, joystick buttons are used in the state machine transition: `M3DemoMachine::GoToNextState` to allow transition by a button press: `OWNER->robot->joystick->isButtonTransition(3)`. Note the use of the ButtonTransition method to avoid capturing repeatedly if the button stays pressed. |
| 126 | + |
| 127 | + |
| 128 | +## Network communication with libFLNL |
| 129 | + |
| 130 | +The M3DemoMachine app is using libFLNL to publish the robot states and read incoming commands over a TCP/IP connection. Together with the use of an FLNLHelper object (`UIserver = new FLNLHelper(robot, "192.168.6.2");`), the library allows the robot state to be sent at every control loop (`UIserver->sendState();` within the `M3DemoMachine::hwStateUpdate(void)` method) and send and process incoming commands. The CORC app is here acting as a server on the specified IP (and port, optional, default is 2048) to which client application can connect to. |
| 131 | + |
| 132 | + |
| 133 | + |
| 134 | +An example class to process incoming states and send/receive commands from a Unity or Matlab script (client side) can be found [here](https://github.com/UniMelbHumanRoboticsLab/CORC-UI-Demo). |
| 135 | + |
0 commit comments