## For instruction on writing tutorials ## http://www.ros.org/wiki/WritingTutorials #################################### ##FILL ME IN #################################### ## for a custom note with links: ## note = ## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links ## note.0= ## descriptive title for the tutorial ## title = Adding Force Control Capabilities ## multi-line description to be displayed in search ## description = With some special settings, you can enable a Cartesian impedance control component of [[hrpsys]] controller called "[[http://fkanehiro.github.io/hrpsys-base/d2/d9f/ImpedanceController.html|Impedance Control]]" on HiroNxo robots, which utilizes the realtime nature of QNX. ## the next tutorial description (optional) ## next = ## links to next tutorial (optional) ## next.0.link= [[rtmros_nextage/Tutorials/Setup ROCON App Manager|Setup ROCON App Manager]] ## next.1.link= ## what level user is this tutorial for ## level= AdvancedCategory ## keywords = force sensor, impedance control, hrpsys controller #################################### <> <> <> = Adding Impedance Control = == Impedance control using hrpsys == Use the output from force/torque sensors attached at the end effectors to achieve arms' compliance. <> Hiro robot reacting using impedance control feature of [[hrpsys]]. More about this video can be found [[http://opensource-robotics.tokyo.jp/?p=1550&lang=en|here]]. To realize the smooth movement, control loop is closed at the low-level controller on `ControlBox`/QNX. This requires sensors connected to ControlBox. === Requirement === * Force/torque sensor, and its driver software that is compatible with QNX 6.5.0. * So far, the large part of this tutorial page is written based on the achievement with the following sensors. No precompiled QNX drivers are provided for neither of them as of June 2016: * [[http://www.jr3.com/products.html|JR3]] device (In Japan, this product is discontinued as of June 2016). * [[http://www.wacoh-tech.com/en/products/dynpick/wdf-6m200-3.html|Dynpick WDF-6M200-3]] (Discontinued) * F/T sensor's '''stable output'''. This is crucial because robot can easily go wild while impedance is on with unstable sensor output! This often requires: * Power supply to the sensor by external source (i.e. not by the ControlBox). * Keep the the sensor's cable away from other cables, esp. those that emit wireless signals. * (For `Dynpick`) By May 2017, QNX 6.5.0 Service Pack 1 is required to establish the connection between the sensor and the host. * Appropriately change `hrpsys` controller on QNX. * QNX developer's license, if aforementioned device driver is not precompiled. Detailed steps for doing this installation is not provided publicly to avoid undesired result and end users are NOT advised to try by themselves. Contacting [[rtmros_nextage/Tutorials/To Contribute Back#Professional_support_services|professional service providers]] is recommended. === Usage: De/Activate impedance feature via Python client === Since Impedance Control feature is deactivated by default, you need to explicitly activate it per each sensor. You can do so via the same Python client that we always use. {{{ $ ipython -i `rospack find hironx_ros_bridge`/scripts/hironx.py : (initialize ipython terminal) IN[1]: robot.startImpedance('rarm') IN[2]: robot.startImpedance('larm') }}} Also you can deactivate it by: {{{ IN[3]: robot.stopImpedance('rarm') IN[4]: robot.stopImpedance('larm') }}} /!\ WARNING: Be sure to '''deactivate Impedance Control feature when the robot is not in use'''! Arms can move based on the sensors output even without any commands passed while Impedance is on. Every day before you activate impedance, check the following minimum set of parameters are fine-tuned. See the following section for the parameters that need to be reconfigured. Some utility functions are defined in [[https://github.com/start-jsk/rtmros_hironx/blob/hydro-devel/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py|hironx_client.py]]. === Impedance Controller Parameters === ==== Minimum set of parameters ==== Arms movement depends on the sensor output, so all users must find the set of parameters that work the best for you. In particular you can start with the following: * "`mass/damper/spring`" defines the stiffness. * reference force to compensate the force in XYZ direction. * Passing this varies per `hrpsys` version, which is explained in the later section. * Setting this 'may' be automated (there's [[https://github.com/start-jsk/rtmros_hironx/pull/498|an open pull request]]). Minimum example (DON'T just use this. You MUST find the values for your robot every day): {{{ robot.startImpedance('larm', M_p = 15.0, D_p = 15.0, K_p = 15.0, M_r = 15.0, D_r = 300.0, K_r = 300.0, ref_force = [-0.56, -0.23, 0.940]) robot.startImpedance('rarm', M_p = 15.0, D_p = 15.0, K_p = 15.0, M_r = 15.0, D_r = 300.0, K_r = 300.0, ref_force = [1.543, 0.13, 2.809]) }}} ==== Full parameters ==== All parameters available are defined in [[http://fkanehiro.github.io/hrpsys-base/d6/d32/structOpenHRP_1_1ImpedanceControllerService_1_1impedanceParam.html|hrpsys doc about ImpedanceControllerService]]. You can set most of those parameters via `startImpedance` method ([[http://docs.ros.org/indigo/api/hironx_ros_bridge/html/classhironx__ros__bridge_1_1hironx__client_1_1HIRONX.html#a8ca9844a8dbe0536185e3d83a7d0aea4|api doc]]), and some can be set via another method, which will be explained later. In the following, for example, `mass/damper/spring` parameters are passed for the left arm. {{{ IN[2]: robot.startImpedance('larm', M_p = 15.0, D_p = 15.0, K_p = 15.0, M_r = 15.0, D_r = 300.0, K_r = 300.0) }}} Parameters for the impedance controller passed via Python I/F here are processed in `ImpedanceController` RT component. For example, reference force mentioned above is processed [[https://github.com/tork-a/hrpsys-base/blob/1c556f35f19d1ae6989cdd6f1fb6395be077d38e/rtc/ImpedanceController/ImpedanceController.cpp#L432-L436|here]] (referring to `hrpsys` 315.1.7). NOTE: Although all users can call the same method `startImpedance`, the arguments slightly vary depending on the version of [[hrpsys]] controller in use. API doc for each version: * [[http://docs.ros.org/indigo/api/hironx_ros_bridge/html/classhironx__ros__bridge_1_1hironx__client_1_1HIRONX.html#ad4770fa0fc1b36a1a298e3f1e16a1105|315.1.x]] * [[http://docs.ros.org/indigo/api/hironx_ros_bridge/html/classhironx__ros__bridge_1_1hironx__client_1_1HIRONX.html#a0e7705625ba35b0ed4b7206175903cc3|315.2.x]] * [[http://docs.ros.org/indigo/api/hironx_ros_bridge/html/classhironx__ros__bridge_1_1hironx__client_1_1HIRONX.html#aca04f04fd0be35cbf0896123c73bc416|315.3.x]] or higher. ==== hrpsys `315.1.x` ==== The method with all available arguments can look like: {{{ startImpedance_315_1(self, arm, M_p = 100.0, D_p = 100.0, K_p = 100.0, M_r = 100.0, D_r = 2000.0, K_r = 2000.0, ref_force = [0, 0, 0], force_gain = [1, 1, 1], ref_moment = [0, 0, 0], moment_gain = [0, 0, 0], sr_gain = 1.0, avoid_gain = 0.0, reference_gain = 0.0, manipulability_limit = 0.1) }}} ==== hrpsys `315.2.x` or higher ==== From 315.2.0 onward, following arguments are dropped and can be set by `robot.seq_svc.setWrenches` instead of `startImpedance` method. See an example at [[https://github.com/fkanehiro/hrpsys-base/pull/434/files#diff-6204f002204dd9ae80f203901f155fa9R44|hrpsys]] sample code: * `ref_force[fx, fy, fz]` (unit: `N`) and `ref_moment[tx, ty, tz]` (unit: `Nm`) can be set via `self.seq_svc.setWrenches`. For example: {{{ self.seq_svc.setWrenches([fx_r, fy_r, fz_r, tx_r, ty_r, tz_r, fx_l, fy_l, fz_l, tx_l, ty_l, tz_l,]) }}} NOTE: In the above example right arm comes first and then left in `setWrenches` arg. This order of sensors is set in the config so can vary per robot. Make sure you know the order (the one who installs this whole thing should tell you about it). ==== Migration from hrpsys `315.1.x`, and `315.2.x` or higher ==== For example, setting reference force must be done as follows: * 315.1.x {{{ robot.startImpedance('larm', ref_force = [-0.56, -0.23, 0.940]) }}} * 315.2.x or higher {{{ robot.startImpedance('larm') robot.seq_svc.setWrenches([0, 0, 0, 0, 0, 0, -0.56, -0.23, 0.940, 0, 0, 0,]) }}} <> === Monitor the force sensor output values === ==== Monitor the output via ROS on Ubuntu ==== You can monitor the F/T sensor's input values via ROS. Be sure you run RTM-ROS bridge (see [[rtmros_nextage/Tutorials/Operating Hiro, NEXTAGE OPEN|a previous tutorial]]): {{{ $ rostopic echo /lhsensor % Left hand $ rostopic echo /rhsensor % Right hand }}} You can plot these same ROS topics on [[rqt_plot]] (e.g. type in "`/lhsensor/wrench/force`" in the topic name text field for the left arm. 2 `rqt_plot` plugins are opened in the following picture.): {{http://wiki.ros.org/rtmros_nextage/Tutorials/Adding%20feedback%20control?action=AttachFile&do=get&target=nxo_dynpick_rqt_plot_20170425.png||width="90%"}} ==== See the impedance parameters currently set ==== You can check the current parameters via `getImpedanceControllerParam` method in Python. Do not forget `ic_svc` object. {{{ $ ipython -i `rospack find nextage_ros_bridge`/script/nextage.py --host nextage : In [3]: robot.startImpedance('larm', M_p = 15.0, D_p = 15.0, K_p = 15.0, M_r = 15.0, D_r = 300.0, K_r = 300.0, ref_force = [-0.55, -0.24, 1.025]) In [5]: robot.ic_svc.getImpedanceControllerParam('larm') Out[5]: (False, OpenHRP.ImpedanceControllerService.impedanceParam(name='', base_name='', target_name='', M_p=0.0, D_p=0.0, K_p=0.0, M_r=0.0, D_r=0.0, K_r=0.0, ref_force=[7.02753183846418e+199, 0.07999999984204262, 0.0], force_gain=[0.0, 0.0, 0.0], ref_moment=[1.0, 1.0, 1.0], moment_gain=[0.0, 0.0, 0.0], sr_gain=0.0, avoid_gain=0.0, reference_gain=0.0, manipulability_limit=0.0)) }}} ==== (Advanced) Monitor the output via the sensor driver directly ==== Only if you have login access to the `ControlBox` (QNX), the following may be an option for you in addition to what's available via ROS. * For Dynpick, you can run the sensor driver's sample value checker: {{{ $ /opt/jsk/bin/sample_dynpick client: msg_no: _IO_MAX + 4 client: msg_reply: 7| client: msg_reply: 7| client: msg_reply: 7| client: msg_reply: 7| client: msg_reply: 1.519 -0.026 1.677 1.093 2.146 -0.381 -0.597 -0.030 1.137 0.079 0.490 -0.513 client: msg_reply: 1.509 -0.029 1.681 1.093 2.143 -0.371 -0.597 -0.027 1.137 0.081 0.492 -0.515 client: msg_reply: 1.504 -0.026 1.677 1.092 2.151 -0.371 -0.600 -0.026 1.131 0.082 0.492 -0.519 : }}} # The order of the output: * For each line, force and rotation about XYZ axes. * Lines are aligned in the order that defined in the robot's 3D model (VRML format) as `sensorID` attribute. === Impedance on simulator === (TBD) === Trouble shooting === ==== {l, r}hsensor not being published? ==== Possible causes: * ROS master may not be running --> Check [[rtmros_nextage/Tutorials/Operating Hiro, NEXTAGE OPEN|a previous tutorial]] to see how to run ROS master. * F/T sensor driver may not be working as expected. * F/T sensor driver may not be connected as expected. Ask [[https://github.com/tork-a/rtmros_nextage/issues|here]] with the output of the following: {{{ $ ssh %YOUR_QNX_USER%@nextage (Password: YOUR_QNX_USER's) $ usb -vvv $ ls -l /dev/ser* $ sloginfo $ ps -ef | grep serusb $ cat /opt/jsk/bin/sample_dynpick $ cat /opt/jsk/bin/run_dynpick.sh }}} == Other than hrpsys == You can also realize force/torque control on Ubuntu machine side if your application doesn't require cut-throat real-time interacton. Some `HiroNxo` users have successfully impelemented [[dynpick_driver|Dynpick force sensor]] ([[http://www.wacoh-tech.com/en/products/dynpick/|Wacoh Tech]]). = Weight offset = The are multiple ways to to cancel the weight offset of the robot's links from force/torque sensor's value. == Using hrpsys == === If weight of eef is known === You can use [[http://fkanehiro.github.io/hrpsys-base/dc/d76/RemoveForceSensorLinkOffset.html|"Remove Force Sensor Link Offset" RTC]] (called `AbsoluteForceSensor` for hrpsys `315.1.x`), along with [[http://fkanehiro.github.io/hrpsys-base/d2/d9f/ImpedanceController.html|"Impedance Control" RTC]]. * [Advantage] This method is not restricted by the orientation of the eef. * You need to pass parameters such as the weight of eef, center of mass, moment of inertia. 1. Check if `Impedance Controller` (`IC`) RTC is enabled on your robot. Run Hiro / NXO python script ([[http://wiki.ros.org/rtmros_nextage/Tutorials/Quick%20Start%20for%20Robot%20Users#Connect_the_operation_command_interface|as you always do]]), then find portions similar to the following: {{{ [rtm.py] Connect sh.rhsensorOut - ic.ref_rhsensorIn [rtm.py] Connect sh.lhsensorOut - ic.ref_lhsensorIn [rtm.py] Connect HiroNX(RobotHardware)0.q - rmfo.qCurrent [rtm.py] Connect rmfo.off_rhsensor - ic.rhsensor [rtm.py] Connect rmfo.off_lhsensor - ic.lhsensor [rtm.py] Connect HiroNX(RobotHardware)0.q - ic.qCurrent [rtm.py] Connect sh.basePosOut - ic.basePosIn }}} `RobotHardware.{r, l}hsensor` should exist. If not, you should look at the return values of `rtls` command, then in the output find `IC` or something similar: {{{ Ubuntu$ rosrun rtshell rtls %HOST_ROBOT%:15005/ }}} Or in newer version of software, {{{ Ubuntu$ rtls %HOST_ROBOT%:15005/ }}} 2. If `IC` RTC is connected to `RMFO` RTC (as the example above does), you can {{{ // Obtaining a structure that stores offset parameters. In [13]: a = robot.rmfo_svc.getForceMomentOffsetParam('lhsensor')[1] // Setting mass to a member variable of the obtained structure. In [14]: a.link_offset_mass = 0.5 // Set the structure back. In [15]: a = robot.rmfo_svc.setForceMomentOffsetParam('lhsensor', a) // If you're curious, this is how the offset parameter structure looks. In [16]: a Out[16]: OpenHRP.RemoveForceSensorLinkOffsetService.forcemomentOffsetParam(force_offset=[0.0, 0.0, 0.0], moment_offset=[0.0, 0.0, 0.0], link_offset_centroid=[0.0, 0.0, 0.0], link_offset_mass=0.0) }}} With the example above, only weight is given for the brevity. It should work but more parameters as noted above are necessary for the better result. === If weight of eef NOT known === With the steps below you can record an offset at an arbitrary pose and then subtract it out. TBD == Using a ROS package == === Use force_torque_tools === [[force_torque_tools]] offers a capability to conduct calibration for your force/torque sensor, and provides a [[force_torque_tools/Tutorials/Force-torque sensor calibration|detailed tutorial]]. Following is the tailored version of instruction for `HiroNXO`, based on the same tutorial. **NOTE-1**: Be sure to consult the original tutorial whenever you have questions; the following is only a supplement to it. **NOTE-2**: As of today (Aug 2015) this is NOT tested with real hardware. Use at your own risk and feedback is welcomed (modifying this wiki is appreciated if you find correct descriptions. Otherwise open a issue ticket at [[https://github.com/tork-a/rtmros_nextage/issues|ROS software repository of NEXTAGE OPEN]]. 1. Copy a config from [[force_torque_sensor_calib]] package. Here we use [[nextage_ros_bridge]] pkg to store the config. {{{ $ roscp force_torque_sensor_calib example_ft_calib_params.yaml `rospack find nextage_ros_bridge`/conf $ roscd nextage_ros_bridge/conf && mv example_ft_calib_params.yaml nxo_ft_calib_params.yaml }}} 2. Edit the config file. {{{ - loop_rate: 650.0 + loop_rate: 1000 }}} At the time of writing, the authoer is thinking of [[dynpick_driver]] as a f/t sensor, which [uses 1,000 hz](https://github.com/tork-a/dynpick_driver/blob/b47b634c4a2b78f69f5d9699bb5c4ce98d44682f/launch/run.launch#L3). Choose appropriate value for your sensor. {{{ moveit_group_name: 'left_arm' + moveit_group_name: 'right_arm' }}} Depending on which arm-tip of NEXTAGE OPEN you attach the f/t sensor, specify the group of MoveIt!. {{{ - random_poses: false + random_poses: true # number of random poses - number_random_poses: 0 + number_random_poses: 30 }}} As the tutorial recommends, better to use random poses unless you have specific reasons to not to. 3. Copy and modify the launch file. {{{ $ roscp force_torque_sensor_calib example_ft_calib.launch `rospack find nextage_ros_bridge`/launch $ roscd nextage_ros_bridge/launch && mv example_ft_calib.launch nxo_ft_calib.launch }}} Edit: {{{ - + }}} 4. Run calibration. {{{ roslaunch nextage_ros_bridge nxo_ft_calib.launch }}} Once again, always go see the [[force_torque_tools/Tutorials/Force-torque sensor calibration|original tutorial]] for the detail. ## AUTOGENERATED DO NOT DELETE ## TutorialCategory ## FILL IN THE STACK TUTORIAL CATEGORY HERE