├── README.md └── images ├── architecture.jpeg ├── drake-visualizer.png ├── drake_arch.png ├── endeffector.png ├── fk_system.png ├── force_ctrl_system.png ├── force_est_system.png ├── ik_system.png ├── jointcontrol.png ├── manipulation_station.png ├── simplediagram.png ├── vel_ctrl_system.png ├── vel_est_system.png └── visualize_system.png /README.md: -------------------------------------------------------------------------------- 1 | # Kuka iiwa and Manipulation using Drake in Python 2 | 3 | This document serves as a quick introduction to Kuka iiwa Robot and controlling it using Drake Python API. The examples are entirely in python and is easy to port to any other robot manipulator. Make sure you have read the official Kuka documentation and is familiar with the functioning before operating the robot. 4 | 5 | The codes of the examples used in this document are available at [https://github.com/achuwilson/pydrake_iiwa](https://github.com/achuwilson/pydrake_iiwa) 6 | 7 | ## **Contents** 8 | - [Introduction](#introduction) 9 | - [Kuka System Architecture](#kuka-system-architecture) 10 | - [Programming the Robot](#programming-the-robot) 11 | - [Kuka Sunrise WorkBench](#kuka-sunrise-workbench) 12 | - [FRI](#fri) 13 | - [Smartpad and Workbench Video Walk-through](#smartpad-and-workbench-video-walk-through) 14 | - [Controlling IIWA from ROS](#controlling-iiwa-from-ros) 15 | - [Controlling IIWA from MATLAB/Simulink/Python](#Controlling-IIWA-from-MATLAB/Simulink/Python) 16 | - [Drake](#drake) 17 | - [Systems](#systems) 18 | - [Diagrams](#diagrams) 19 | - [Context](#context) 20 | - [MultibodyPlant](#multibodyplant) 21 | - [Tutorials](#tutorials) 22 | - [LCM](#lcm) 23 | - [Controlling IIWA from Drake](#controlling-iiwa-from-drake) 24 | - [Drake IIWA Java Application](#drake-iiwa-java-application) 25 | - [kuka_driver](#kuka_driver) 26 | - [IIWA-LCM Interface](#iiwa-lcm-interface) 27 | - [Controlling Custom Robot from Drake](#Controlling-Custom-Robot-from-Drake) 28 | - [Manipulation Station](#manipulation-station) 29 | - [Examples](#Examples) 30 | - [Joint Control](#joint-control) 31 | - [Visualizing the robot state in Drake visualizers](#visualizing-the-robot-state-in-Drake-visualizers) 32 | - [Adding an end effector](#Adding-an-end-effector) 33 | - [Forward Kinematics](#forward-kinematics) 34 | - [Inverse Kinematics](#inverse-inematics) 35 | - [Estimating Cartesian Velocities](#estimating-cartesian-velocities) 36 | - [Cartesian velocity control](#Cartesian-velocity-control) 37 | - [Estimating Cartesian forces](#estimating-cartesian-forces) 38 | - [Hybrid Force-Position control](#hybrid-force-position-control) 39 | - [Motion Planning and Collision Avoidance](#motion-planning-and-collision-avoidance) 40 | - [Gravity Compensation](#gravity-compensation) 41 | - [Haptic Force Feedback](#) 42 | 43 | 44 | 45 | ## **Introduction** 46 | 47 | 48 | Kuka LBR IIWA is a 7 DOF collaborative robot manipulator which has got excellent torque control capabilities in addition to the common position control features. This enables capabilities like impedance control which is much beneficial when the robot has to interact with noisy environment models, where pure position control can break things (or even the robot). The robot has joint torque sensors on all the joints. ( NOTE: the torque estimation is not as accurate as an external Force-Torque sensor. It is observed that the error depends on robot pose and has an accuracy of approximately 5N) 49 | 50 | ## **Kuka System Architecture** 51 | 52 | A typical architecture of the manipulation station will be as follows: 53 | 54 | ![](images/architecture.jpeg) 55 | 56 | IIWA manipulator is controlled by Kuka Sunrise Cabinet controller. It has an industrial PC running Kuka's version of Windows CE called Sunrise OS and a realtime OS. The sunrise OS handles the user program, GUI etc and is accessible to the user. The realtime part is hidden from the user and controls the low level hardware interfaces. 57 | 58 | The Sunrise Cabinet have multiple Ethernet interfaces. Kuka Line Interface (KLI) (Port X66) is the only one enabled by default and is used to connect the Controller to an external PC. Kuka provides Sunrise Workbench IDE with which the IIWA could be programmed in Java. The IDE also helps installing software packages and configuring settings such as network and safety configurations, input-output etc. 59 | 60 | The other interface called Fast Robot Interface (FRI) enables access to the robot controller from an external computer in real time. It operates over the Kuka Optional Network Interface (KONI) and has to be enabled by installing the FRI package. Drake uses this interface to interface with the IIWA hardware. 61 | 62 | Additional interfaces like EtherCAT, PROFINET are also available, which can be enabled by installing the corresponding software package from Kuka. 63 | 64 | Kuka Smartpad, the handheld controller allows to start and stop the programs loaded into the SunriseCabinet. In fact, it shows the remote desktop view of the Sunrise OS running inside the Cabinet. Connecting an external monitor to the DVI port on the back side of the Sunrise Cabinet also shows the same Smartpad GUI. In addition, it is also possible to access the Smartpad GUI using Remote Desktop tools over the KLI ethernet port. 65 | 66 | The following are the default IP address of the Ethernet Ports: 67 | 68 | KLI : 172.31.1.147 69 | KONI FRI : 192.170.10.2 70 | 71 | Remote Desktop: 172.31.1.147 72 | Username: KukaUser 73 | Password: 68kuka1secpw59 74 | 75 | 76 | The following documents give a detailed overview of the Kuka IIWA Robot systems. It is recommended to get familiarized with the IIWA system from the following documents before operating the robot. 77 | - [KUKA Sunrise.OS 1.16, Operating Instructions for End Users](https://indianinstituteofscience-my.sharepoint.com/:b:/g/personal/achuwilson_iisc_ac_in/EV4iYsOWqzJDo67tXQCS5RkBYui1geiQtkUp61vTxEKwrA) 78 | - [KUKA Sunrise Cabinet Operating Instructions](https://indianinstituteofscience-my.sharepoint.com/:b:/g/personal/achuwilson_iisc_ac_in/Edw4l1pf6npHoR7z2O2gx-IB9v7VA7hakrdIowxQbYPMbA?e=wyLi8R) 79 | - [System Software KUKA Sunrise.OS 1.16, KUKA Sunrise.Workbench 1.16, Operating and Programming Instructions for System Integrators](https://indianinstituteofscience-my.sharepoint.com/:b:/g/personal/achuwilson_iisc_ac_in/ETb2S5FZac5DiL733qOmhicB3BOZzJFAMyjdIQaC3mI6rA?e=9Ne5Gq). This document has Java API documentation 80 | 81 | NOTE: the linked documents could not be shared publicly as it would violate Kuka's Copyright notice and require IISc login. You could also create a free account on [Kuka Xpert](https://xpert.kuka.com) to download these 82 | 83 | 84 | ## **Programming the Robot** 85 | 86 | Kuka basically provides the following two methods to program the robot 87 | - Kuka Sunrise Workbench 88 | - FRI 89 | 90 | ### **Kuka Sunrise Workbench** 91 | 92 | 93 | 94 | The default programming option provided by Kuka is through its Java APIs using Sunrise Workbench, which is in-fact a customized Eclipse IDE. It is not available for download on the Kuka website, as it has to match the version of the Sunrise OS running on the controller, So request for your copy of Sunrise Workbench to your Kuka robot supplier. 95 | 96 | The Java APIs may differ slightly depending upon the version of the Sunrise OS and Workbench that is being used. It is available in the [Kuka Sunrise Workbench Operating and Programming Instructions](https://indianinstituteofscience-my.sharepoint.com/:b:/g/personal/achuwilson_iisc_ac_in/ETb2S5FZac5DiL733qOmhicB3BOZzJFAMyjdIQaC3mI6rA?e=9Ne5Gq) 97 | 98 | After developing an application in the Sunrise Workbench, the user has to synchronize it with the Sunrise OS in the controller. This just copies the project files to the controller over the KLI port. 99 | 100 | Download Links ( Require IISc login) 101 | - [Kuka Sunrise Workbench v1.16](https://indianinstituteofscience-my.sharepoint.com/:u:/g/personal/achuwilson_iisc_ac_in/EVTSc15X_GRIlWSK5q1Tb30Bb0Yqsdy8eesSHTUHCa5uRg?e=tj55YR) 102 | - [FRI Addon](https://indianinstituteofscience-my.sharepoint.com/:u:/g/personal/achuwilson_iisc_ac_in/EXaiVwwBq4pKsusDc46TTOQB8VykoFdGvrw-m9PZhETiDA?e=HIVX5J) 103 | - [SmartServo Addon](https://indianinstituteofscience-my.sharepoint.com/:u:/g/personal/achuwilson_iisc_ac_in/EcM-n6luAApEnvS9MxJgs4YBfm4CiQUMdh_WkRj2hqixMQ?e=tkKpXC) 104 | - [HRC Addon](https://indianinstituteofscience-my.sharepoint.com/:u:/g/personal/achuwilson_iisc_ac_in/EY7uGq3RELJAsfiPXchYBXYBTzac1upeZKnQWZ3S3OYYxg?e=Vih3Jo) 105 | - [Sample Sunrise Project](https://indianinstituteofscience-my.sharepoint.com/:u:/g/personal/achuwilson_iisc_ac_in/EV-CtR-rhjZLkxZBV-kK0oMBhW0X3Rcw1UlBOHzYOXZxNw?e=ddkZqd) 106 | - [FRI Client SDK C++](https://indianinstituteofscience-my.sharepoint.com/:u:/g/personal/achuwilson_iisc_ac_in/ES0_qKtT5HNLqHYRvyvdlD8BOUA83MfFFzkkU1Ie0pmPLw?e=C48Smz) 107 | 108 | NOTE: While creating a new project and synchronizing it with the Controller, after changing any safety related settings, the SmartPad would show a "safety configuration not activated" error. The default password to activate the safety configuration is ```ARGUS``` 109 | 110 | 111 | After loading them, applications can be selected and executed using the 112 | Smartpad interface. The Key on the Smartpad helps switch between ```AUT``` - automatic and ```T1``` - reduced velocity mode. 113 | 114 | ### **FRI** 115 | 116 | FRI stands for "Fast Robot Interface", which is an addon provided by Kuka, enables real time control of the robot system. This requires control signals be generated in an external computer and sent over the KONI Ethernet port. The FRI is not enabled out of the box and has to be installed and enabled through the Sunrise workbench. The default IP address of the FRI interface is ```192.170.10.2``` 117 | 118 | Kuka provides FRI-Client libraries in C++ and Java, which can be found inside the examples directory after the installation of FRI library in Sunrise WorkBench. The C++ libraries can be found in the file named ```FRI-Client-SDK_Cpp.zip```. It can be used to build applications that communicates with Kuka controller over FRI. 119 | 120 | Drake uses the FRI interface to control the IIWA from an external computer. 121 | 122 | ## **Smartpad and Workbench Video Walk-through** 123 | TODO #1 124 | ## **Controlling IIWA from ROS** 125 | 126 | The [```iiwa_stack```](https://github.com/IFL-CAMP/iiwa_stack) package can be used to interface IIWA from ROS. It uses the Smart Servoing functionality over the KLI network interface. 127 | 128 | The ROSJava nodes running on the robot controller as a Sunrise RobotApplication sends data and receives commands from a ROS master running on the external PC. The [wiki](https://github.com/IFL-CAMP/iiwa_stack/wiki) provides detailed instructions on controlling from ROS. 129 | 130 | ## **Controlling IIWA from MATLAB/Simulink/Python** 131 | - The [Kuka Sunrise Toolbox for Matlab](https://github.com/Modi1987/KST-Kuka-Sunrise-Toolbox) allows control of the LBR iiwa robot from MATLAB. 132 | - The [Simulink-iiwa interface] (https://github.com/Modi1987/Simulink-iiwa-interface) for Simulink based control. 133 | - [iiwaPy](https://github.com/Modi1987/iiwaPy) can be used for control of the iiwa from Python. 134 | 135 | All of the above packages are based on the Kuka Sunrise Toolbox for Matlab. 136 | 137 | 138 | 139 | # **Drake** 140 | [Drake](https://drake.mit.edu/) is a toolbox which can model dynamic systems, solve mathematical problems and has built in multibody kinematics and dynamics. 141 | 142 | Running the examples requires installation of Python bindings of Drake as documented [here](https://drake.mit.edu/python_bindings.html#python-bindings-binary). These were tested in Ubuntu 18.04 with binary installation of Drake. 143 | 144 | 145 | ### **Systems** 146 | The basic building block of Drake is a ```System```, which has input and output ports as well as an optional state. Multiple systems can be interconnected either as a ```Diagram``` or ```LeafSystem```. LeafSystems are the minimum building block and is often used for basic components like sensors, actuators, controllers, planners etc, which has a specific functionality. Drake come with many built-in systems which can be found in the [official documentation](https://drake.mit.edu/doxygen_cxx/group__systems.html) 147 | 148 | ### **Diagrams** 149 | 150 | Diagrams consists of multiple LeafSystems or even other Diagrams inside and are used to represent a set of interconnected systems that function as a whole. 151 | 152 | [```example_drake_simplediagram.py```](https://github.com/achuwilson/pydrake_iiwa/blob/main/example_drake_simplediagram.py) creates a simple diagram which looks as follows: 153 | 154 | ![](images/simplediagram.png) 155 | 156 | The built-in ```SystemSlider``` is used to create a GUI with 3 sliders, whose output is fed into a ```PrintSystem``` which evaluates and prints the input values to terminal at a specific update rate. 157 | 158 | ### **Context** 159 | 160 | All ```Diagram``` and ```System``` has a ```Context``` which embodies the state and parameters of the system. In addition to the ```Context``` of the main diagram, each subsystems and sub diagrams have their own unique context with with we can interact with the internals of the systems. Given the ```context```, all methods called on a Diagram or System is deterministic and repeatable. The ```Simulator``` needs the ```Diagram``` and its ```Context```for running the computations. 161 | 162 | ### **MultibodyPlant** 163 | 164 | ```MultibodyPlant``` is one of the most important built-in systems that Drake provides. It is used to represent multiple rigid bodies connected in tree, a common practice with serial robot manipulators. It internally uses rigid body tree algorithms to compute the kinematics. ```MultibodyPlant``` also has both inputs and outputs which could be connected to other systems such as controllers or visualizers. 165 | 166 | ### **Tutorials** 167 | Drake provides a set of [tutorials](https://github.com/RobotLocomotion/drake/tree/master/tutorials) 168 | 169 | 170 | [```dynamical_systems.ipynb```](https://github.com/RobotLocomotion/drake/blob/master/tutorials/dynamical_systems.ipynb) gives an introduction to modelling systems in Drake 171 | 172 | [```mathematical_program.ipynb```](https://github.com/RobotLocomotion/drake/blob/master/tutorials/mathematical_program.ipynb) introduces numerical programming capabilities of Drake 173 | 174 | 175 | ## **LCM** 176 | 177 | [LCM](https://lcm-proj.github.io/) stands for Lightweight Communications and Marshalling. It is a set of libraries that can provide publish/subscribe message passing capabilities. 178 | 179 | LCM implementatoions are available for all common programming languages and operating systems. Refer to the [LCM example](https://lcm-proj.github.io/tutorial_general.html) for a quick-start. 180 | 181 | #### **Plotting LCM Messages** 182 | Drake includes ```drake-lcm-spy``` in ```/opt/drake/bin``` to plot and visualize LCM messages. 183 | 184 | ## **Controlling IIWA from Drake** 185 | 186 | The following diagram shows a typical architecture of a Drake based system for controlling the iiwa. 187 | 188 | ![](images/drake_arch.png) 189 | 190 | 191 | ```DrakeFRIPositionDriver``` and ```DrakeFRITorqueDriver``` are Java applications built using the Sunrise Workbench and running inside the Sunrise Controller. They open an FRI connection at a specified network port, to which an external computer can connect to. 192 | 193 | ```kuka_driver``` is a C++ application built using the FRI-Client-SDK-Cpp and runs on the external computer. It communicates with the Sunrise Controller over the FRI/KONI interface. It also publishes and subscribes LCM messages which can be used by other programs to read/write data to the iiwa robot. 194 | 195 | Drake has a built in ```LCMInterfaceSystem``` which allows drake systems to publish and subscribe to LCM messages. Other Drake systems make use of these systems to communicate with the hardware. 196 | 197 | ### **Drake IIWA Java Application** 198 | The Java application runs on the on the Sunrise Controller and opens an FRI connection to which the ```kuka_driver``` running on an external computer connects to. 199 | 200 | 201 | The detailed documentation and code is available in [```drake-iiwa-driver```](https://github.com/RobotLocomotion/drake-iiwa-driver) 202 | There are two applications 203 | 204 | - DrakeFRIPositionDriver 205 | - DrakeFRITorqueDriver 206 | 207 | The DrakeFRIPositionDriver, as the name implies allows controlling the robot in position control mode, taking in joint position commands. 208 | 209 | The DrakeFRITorqueDriver allows for the control of the robot in impedance control mode and takes in joint position as well as joint feed-forward torque commands. We would be using this mode more often. 210 | 211 | Both the drivers output robot status like joint positions, velocities, torques etc 212 | 213 | 214 | ### **kuka_driver** 215 | The ```kuka_driver``` runs on the external computer, connects to the Java application running on the robot and provides an LCM interface to read/write data. 216 | 217 | It has to be compiled as in this [documentation](https://github.com/RobotLocomotion/drake-iiwa-driver/blob/master/README.md) and requires FRI client SDK for compilation. 218 | 219 | After compilation, the ```kuka_driver``` should be run first, so as to communicate with IIWA 220 | 221 | 222 | ### **IIWA-LCM Interface** 223 | ```kuka_driver``` provides read/write interface to the IIWA through LCM messages. It generates three LCM message channels 224 | - ```IIWA_STATUS``` of the type ```lcmt_iiwa_status```, defined in [```lcmt_iiwa_status.lcm```](https://github.com/RobotLocomotion/drake/blob/master/lcmtypes/lcmt_iiwa_status.lcm) 225 | - ```IIWA_COMMAND``` of the type ```lcmt_iiwa_command```, defined in [```lcmt_iiwa_command.lcm```](https://github.com/RobotLocomotion/drake/blob/master/lcmtypes/lcmt_iiwa_command.lcm) 226 | - ```IIWA_STATUS_TELEMETRY``` of the type ```lcmt_iiwa_status_telemetry```, defined in [```lcmt_iiwa_status_telemetry.lcm```](https://github.com/RobotLocomotion/drake/blob/master/lcmtypes/lcmt_iiwa_status_telemetry.lcm) 227 | 228 | By default, ```kuka_driver``` publishes/ subscribes these messages at 200Hz 229 | 230 | ```IIWA_STATUS``` provides the robot joint status which includes joint position, velocities and torques. An example which subscribes to the ```IIWA_STATUS``` and prints the output is available in [```lcm_examples/iiwa-lcm-listener.py```](https://github.com/achuwilson/pydrake_iiwa/blob/main/lcm_examples/iiwa-lcm-listener.py) 231 | 232 | ```IIWA_COMMAND``` is used to command joint positions with an optional feed forward joint torque. An example which subscribes to ```IIWA_STATUS``` to estimate the current robot configuration and move joint 7 incrementally is available in [```lcm_examples/iiwa-lcm-publisher.py```](https://github.com/achuwilson/pydrake_iiwa/blob/main/lcm_examples/iiwa-lcm-publisher.py) 233 | 234 | ```IIWA_STATUS_TELEMETRY``` provides timing information, which can be used to estimate the latency in the FRI communication between the external computer and the robot controller. 235 | 236 | ## **Controlling Custom Robot from Drake** 237 | The Kuka iiwa interface in this documentation can be adapted to interface custom robot manipulators. Following would be the minimum requirements: 238 | - a hardware interface program similar to ```kuka_driver```, which reads and writes from the hardware. As a bare minimum, we should be able to write joint positions and read joint positions from the hardware and pass it on as LCM messages. 239 | - Define custom LCM messages depending on the hardware capabilities. Take a look at [```lcmt_iiwa_command.lcm```](https://github.com/RobotLocomotion/drake/blob/master/lcmtypes/lcmt_iiwa_command.lcm) and [```lcmt_iiwa_status.lcm```](https://github.com/RobotLocomotion/drake/blob/master/lcmtypes/lcmt_iiwa_status.lcm) and others defined in [lcmtypes](https://github.com/RobotLocomotion/drake/tree/master/lcmtypes) 240 | - Develop Drake systems that parses the custom LCM messages and interfaces them to other Drake systems, similar to [```iiwa_status_receiver.py```](https://github.com/achuwilson/pydrake_iiwa/blob/main/iiwa_command_sender.py) and [```iiwa_command_sender.py```](https://github.com/achuwilson/pydrake_iiwa/blob/main/iiwa_command_sender.py). 241 | - URDF/SDF model of the robot. This will be used to create the MultibodyPlant which is used to compute the kinematics and dynamics of the system. (NOTE: use OBJ files instead of STL/DAE) 242 | 243 | ## **Manipulation Station** 244 | The manipulation station consists of the IIWA robot, the Drake systems required to communicate and parse the data with the IIWA as well as other optional hardware such as cameras, grippers etc 245 | 246 | The ```IiwaManipulationStation```, defined in [```iiwa_manipulation_station.py```](https://github.com/achuwilson/pydrake_iiwa/blob/main/iiwa_manipulation_station.py) is a ```Diagram``` which has to be included and initialized in the diagram created by the user, so as to communicate with the IIWA. It consists of the following systems: 247 | 248 | - LcmInterfaceSystem 249 | - LcmSubscriberSystem 250 | - LcmPublisherSystem 251 | - IiwaCommandSender 252 | - IiwaStatusReceiver 253 | - MultibodyPlant 254 | 255 | 256 | The diagram of the manipulation system looks as follows: 257 | 258 | ![](images/manipulation_station.png) 259 | 260 | 261 | The ```LcmInterfaceSystem``` has no inputs nor outputs nor state nor parameters; it declares only an update event that pumps LCM messages into their subscribers if the LCM stack has message(s) waiting. The subscribers will then update their outputs using their own declared events 262 | 263 | The ```LcmSubscriberSystem``` subscribes to the LCM data stream and outputs the received data through a single output port. 264 | 265 | The ```LCMPublisherSystem``` has a single input port and outputs the received data to the LCM data stream at a specified update rate. 266 | 267 | 268 | The ```IiwaStatusReceiver```, defined in [```iiwa_status_receiver.py```](https://github.com/achuwilson/pydrake_iiwa/blob/main/iiwa_status_receiver.py) parses the ```IIWA_STATUS``` LCM message into the following vector valued outputs: 269 | 270 | - position_commanded 271 | - position_measured 272 | - velocity_estimated 273 | - torque_commanded 274 | - torque_measured 275 | - torque_external 276 | An example on using ```IiwaStatusReceiver``` is available in [```example_iiwa_status_receiver.py```](https://github.com/achuwilson/pydrake_iiwa/blob/main/example_iiwa_status_receiver.py) 277 | 278 | 279 | The ```IiwaCommandSender```, defined in [```iiwa_command_sender.py```](https://github.com/achuwilson/pydrake_iiwa/blob/main/iiwa_command_sender.py) encodes the input into ```IIWA_COMMAND``` LCM message which is published by the ```LcmSubscriberSystem```. It has the following two vector valued inputs accepting vectors of size 7. 280 | 281 | - position 282 | - torque 283 | 284 | An example usage of ```IiwaCommandSender``` is available in [```example_iiwa_command_sender.py```](https://github.com/achuwilson/pydrake_iiwa/blob/main/example_iiwa_command_sender.py). Be EXTREMELY CAUTIOUS before running this example, as it would instantaneously command 0 positions to joints. Don't run this example unless all joints are near to zero position. 285 | 286 | The ```MultibodyPlant``` models the external plant that has to be controlled and helps in the computation of its kinematics and dynamics. 287 | 288 | 289 | The input and output ports of the individual systems inside inside a diagram has to be exported to outside so that other drake systems can interface with the inner systems. The ```ExportOutput``` and ```ExportInput``` methods of ```DiagramBuilder``` are used for this. 290 | 291 | # **Examples** 292 | The codes of the examples used in this document are available at [https://github.com/achuwilson/pydrake_iiwa](https://github.com/achuwilson/pydrake_iiwa) 293 | 294 | To run the examples, follow these steps 295 | - STEP1: Make sure that the Kuka is in ```AUT ``` mode in SmartPad 296 | - STEP2: Select and run ```DrakeFRITorqueDriver``` Application from the Smartpad 297 | - STEP3: run ```kuka_driver``` in the PC. This will cause the brakes on IIWA to release 298 | - STEP4: (optional) Start ```drake-visualizer``` or ```meshcat-server``` for examples requiring visualization 299 | - STEP4: run the example 300 | ## **Joint Control** 301 | [```example_joint_slider.py```](https://github.com/achuwilson/pydrake_iiwa/blob/main/example_joint_slider.py) 302 | 303 | In this simple example, we make use of the drake ```JointSliders``` system to control the joint values of the robot. 304 | The system diagram of the example is as follows: 305 | 306 | ![](images/jointcontrol.png) 307 | 308 | The output port of the ```JointSliders``` system is connected through a ```FirstOrderLowPassFilter``` to the ```iiwa_position``` port of the ```IiwaHardwareInterface``` manipulation station. The ```FirstOrderLowPassFilter``` helps to smoothen the motion by filtering out high frequency changes in position which may cause jerk. 309 | 310 | 311 | ## **Visualizing the robot state in Drake visualizers** 312 | [```example_iiwa_visualize.py```](https://github.com/achuwilson/pydrake_iiwa/blob/main/example_iiwa_visualize.py) 313 | 314 | Drake has multiple visualizers and uses the SceneGraph system to output the visualizations. By default, Drake comes with a VTK based visualizer which is located in ```/opt/drake/bin/drake-visualizer```. We have to launch the visualizer before running the simulation. 315 | 316 | Drake also has a Meshcat based visualizer which can display the output in a browser window. Run ```meshcat-server``` present in the same directory. Meshcat visualizer is greatly helpful when running Drake as IPython notebooks in Google Colab 317 | 318 | The system diagram of the example is as follows: 319 | 320 | ![](images/visualize_system.png) 321 | 322 | In the [```example_iiwa_visualize.py```](https://github.com/achuwilson/pydrake_iiwa/blob/main/example_iiwa_visualize.py) example, the ```MultibodyPositionToGeometryPose``` system takes in the joint positions of the robot and outputs the pose output required by the ```SceneGraph``` system. The ```DrakeVisualizer``` and ```meshcat_visualizer``` queries the scenegraph system and updates the rendering. 323 | 324 | The DrakeVisualizer looks as follows: 325 | 326 | ![](images/drake-visualizer.png) 327 | 328 | ## **Adding an end effector** 329 | [```example_iiwa_endeffector.py```](https://github.com/achuwilson/pydrake_iiwa/blob/main/example_iiwa_endeffector.py) 330 | 331 | This example demonstrates how to add a custom end-effector/gripper to the ```IiwaManipulationStation```. End effector models, either in URDF or SDF format could be imported and added to the ```MultibodyPlant``` before finalizing it. It also needs to be welded to the last link of IIWA. 332 | 333 | Following image shows IIWA attached with the small blue finger defined in [```models/onefinger.urdf```](https://github.com/achuwilson/pydrake_iiwa/blob/main/models/onefinger.urdf): 334 | 335 | ![](images/endeffector.png) 336 | 337 | Once the end-effector is added to the MultibodyPlant, we can refer to it by the name defined in the URDF file. 338 | 339 | ## **Forward Kinematics** 340 | 341 | [```example_FK.py```](https://github.com/achuwilson/pydrake_iiwa/blob/main/example_FK.py) 342 | 343 | Forward Kinematics calculates the position of end effector/gripper in the world given the joint position values. 344 | 345 | ![](images/fk_system.png) 346 | 347 | In this example, the low pass filtered values from the slider is used to set the joint positions of the IIWA hardware. The ```iiwa_position_measured``` output port of the manipulation station is connected to the ```FKSystem```, where the joint values of the multibodyplant are updated. The ```EvalBodyPoseInWorld``` function can be used to evaluate the position of the body in the world. 348 | 349 | 350 | ## **Inverse Kinematics** 351 | [```example_IK.py```](https://github.com/achuwilson/pydrake_iiwa/blob/main/example_IK.py) 352 | 353 | Inverse Kinematics solves for the joint positions required to reach a particular end-effector pose. 354 | 355 | Drake has a numeric [InverseKinematics](https://drake.mit.edu/doxygen_cxx/classdrake_1_1multibody_1_1_inverse_kinematics.html) solver which formulates IK as a nonlinear optimization problem. We can specify non linear inequality constraints like minimum distance between bodies, position/orientation constraints, target gaze constraints etc. 356 | Refer to [MIT 6.881 Lecture 15, Motion Planning, Part 1](https://www.youtube.com/watch?v=RjKkA_6-0C4) for more insights on Inverse kinematics and declaring constraints. Corresponding [IPython Notebook](https://github.com/RussTedrake/manipulation/blob/master/trajectories.ipynb) 357 | 358 | Drake also has a [Differential Inverse Kinematics solver](https://drake.mit.edu/doxygen_cxx/namespacedrake_1_1manipulation_1_1planner.html), which calculates joint velocities using Jacobian and integrates it to calculate the joint position. The [```example_IK.py```](https://github.com/achuwilson/pydrake_iiwa/blob/main/example_IK.py) uses the differential IK method implemented in [```differential_ik.py```](https://github.com/RobotLocomotion/drake/blob/master/examples/manipulation_station/differential_ik.py) 359 | 360 | The system diagram of the [```example_IK.py```](https://github.com/achuwilson/pydrake_iiwa/blob/main/example_IK.py) is as follows: 361 | 362 | ![](images/ik_system.png) 363 | 364 | 365 | ## **Estimating Cartesian Velocities** 366 | [```example_velocity_estimate.py```](https://github.com/achuwilson/pydrake_iiwa/blob/main/example_velocity_estimate.py) 367 | 368 | End effector velocities can be estimated by multiplying the robot Jacobian with joint velocities. Drake ```MultibodyPlant``` has the ```CalcJacobianSpatialVelocity``` method, which could be used to calculate the Spatial Jacobian. 369 | 370 | The system diagram of the example is as follows: 371 | 372 | ![](images/vel_est_system.png) 373 | 374 | In [```example_velocity_estimate.py```](https://github.com/achuwilson/pydrake_iiwa/blob/main/example_velocity_estimate.py), The output of ```EndEffectorTeleop``` is used to control the cartesian end effector position through the ```DifferentialIK``` system. The ```iiwa_velocity_estimated``` and ```iiwa_position_measured``` outputs of the manipulation station are used by the ```velocityEstimator``` system to calculate the Jacobian and corresponding end effector velocities. 375 | 376 | ## **Cartesian velocity control** 377 | [```example_velocity_control.py```](https://github.com/achuwilson/pydrake_iiwa/blob/main/example_velocity_control.py) 378 | 379 | WARNING: Pay close attention when running this example on the real robot. The end effector keeps on moving at the commanded velocity, until the slider is moved back to zero. 380 | 381 | Joint velocities required to move the end effector at a desired velocity in cartesian space are computed using an inverse Jacobian controller. 382 | 383 | The system diagram is as follows: 384 | 385 | ![](images/vel_ctrl_system.png) 386 | 387 | The desired end effector velocity from the slider and the current joint position is fed as inputs to the ```PseudoInverseVelocityController```. It calculates the required joint velocities, which are integrated and fed as ```iiwa_position``` input. 388 | 389 | ## **Estimating Cartesian forces** 390 | [```example_force_estimate.py```](https://github.com/achuwilson/pydrake_iiwa/blob/main/example_force_estimate.py) 391 | 392 | IIWA has joint torque sensors at all the 7 joints. This example uses jacobian transpose to estimate the forces and in cartesian space at the end effector from the measured joint torques. 393 | 394 | The system diagram of the example is as follows: 395 | 396 | ![](images/force_est_system.png) 397 | 398 | It looks similar to the velocity estimation system, except that in this case, there is a ```forceEstimator``` system that takes in ```iiwa_position_measured``` and ```iiwa_torque_external``` 399 | 400 | ## **Hybrid Force-Position control** 401 | [```example_force_feedforward.py```](https://github.com/achuwilson/pydrake_iiwa/blob/main/example_force_feedforward.py) 402 | 403 | In Hybrid force-position demo, the end effector is able to move to/maintain a position as well exert force in arbitrary directions. 404 | 405 | We make use of the ```iiwa_feedforward_torque``` input to provide additional joint torques. These torques are calculated using Jacobian transpose pseudo-inverse in the ```FeedForwardForceController``` system. while running the demo, two windows will pop up, one for controlling the cartesian position and the other for the cartesian wrench. 406 | 407 | The system diagram of the example is as follows: 408 | 409 | ![](images/force_ctrl_system.png) 410 | 411 | ## **Motion Planning and Collision Avoidance** 412 | TODO #2 413 | ## **Gravity Compensation** 414 | TODO #3 415 | ## **Haptic Force Feedback** 416 | TODO #4 -------------------------------------------------------------------------------- /images/architecture.jpeg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/achuwilson/pydrake-manipulator-documentation/359cb964e67e038fc0a6f10bf59f8b92ef975072/images/architecture.jpeg -------------------------------------------------------------------------------- /images/drake-visualizer.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/achuwilson/pydrake-manipulator-documentation/359cb964e67e038fc0a6f10bf59f8b92ef975072/images/drake-visualizer.png -------------------------------------------------------------------------------- /images/drake_arch.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/achuwilson/pydrake-manipulator-documentation/359cb964e67e038fc0a6f10bf59f8b92ef975072/images/drake_arch.png -------------------------------------------------------------------------------- /images/endeffector.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/achuwilson/pydrake-manipulator-documentation/359cb964e67e038fc0a6f10bf59f8b92ef975072/images/endeffector.png -------------------------------------------------------------------------------- /images/fk_system.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/achuwilson/pydrake-manipulator-documentation/359cb964e67e038fc0a6f10bf59f8b92ef975072/images/fk_system.png -------------------------------------------------------------------------------- /images/force_ctrl_system.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/achuwilson/pydrake-manipulator-documentation/359cb964e67e038fc0a6f10bf59f8b92ef975072/images/force_ctrl_system.png -------------------------------------------------------------------------------- /images/force_est_system.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/achuwilson/pydrake-manipulator-documentation/359cb964e67e038fc0a6f10bf59f8b92ef975072/images/force_est_system.png -------------------------------------------------------------------------------- /images/ik_system.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/achuwilson/pydrake-manipulator-documentation/359cb964e67e038fc0a6f10bf59f8b92ef975072/images/ik_system.png -------------------------------------------------------------------------------- /images/jointcontrol.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/achuwilson/pydrake-manipulator-documentation/359cb964e67e038fc0a6f10bf59f8b92ef975072/images/jointcontrol.png -------------------------------------------------------------------------------- /images/manipulation_station.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/achuwilson/pydrake-manipulator-documentation/359cb964e67e038fc0a6f10bf59f8b92ef975072/images/manipulation_station.png -------------------------------------------------------------------------------- /images/simplediagram.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/achuwilson/pydrake-manipulator-documentation/359cb964e67e038fc0a6f10bf59f8b92ef975072/images/simplediagram.png -------------------------------------------------------------------------------- /images/vel_ctrl_system.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/achuwilson/pydrake-manipulator-documentation/359cb964e67e038fc0a6f10bf59f8b92ef975072/images/vel_ctrl_system.png -------------------------------------------------------------------------------- /images/vel_est_system.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/achuwilson/pydrake-manipulator-documentation/359cb964e67e038fc0a6f10bf59f8b92ef975072/images/vel_est_system.png -------------------------------------------------------------------------------- /images/visualize_system.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/achuwilson/pydrake-manipulator-documentation/359cb964e67e038fc0a6f10bf59f8b92ef975072/images/visualize_system.png --------------------------------------------------------------------------------