arntanguy · TsuruMasato · Jan 12, 2022 · Jan 13, 2022 · Jan 13, 2022 · Jan 13, 2022
diff --git a/README.md b/README.md
@@ -1,56 +1,177 @@
-# LIPM Walking Controller
+# Re-organized LIPM Walking Controller for Joystick Walking
 
 [![License](https://img.shields.io/badge/License-BSD%202--Clause-green.svg)](https://opensource.org/licenses/BSD-2-Clause)
 [![CI](https://github.com/jrl-umi3218/lipm_walking_controller/workflows/CI/badge.svg?branch=topic/ci)](https://github.com/jrl-umi3218/lipm_walking_controller/actions?query=workflow%3A%22CI%22)
 [![Documentation](https://img.shields.io/badge/doxygen-online-brightgreen?logo=read-the-docs&style=flat)](http://jrl-umi3218.github.io/lipm_walking_controller/doxygen/HEAD/index.html)
 
-[![Stair climbing by the HRP-4 humanoid robot](https://scaron.info/images/stair-climbing.jpg)](https://www.youtube.com/watch?v=vFCFKAunsYM&t=22)
+[![demo video](https://github.com/TsuruMasato/lipm_walking_controller/blob/rebase_stabilizer_ana/image/Screenshot%20from%202022-01-18%2019-17-45.png)](https://youtu.be/XoXDZBgbamc)
 
-Source code of the walking and stair climbing controller used in the experiments of [Stair Climbing Stabilization of the HRP-4 Humanoid Robot using Whole-body Admittance Control](https://hal.archives-ouvertes.fr/hal-01875387/document), as well as in an industrial demonstrator at the [Airbus Saint-Nazaire factory](https://cordis.europa.eu/project/rcn/194280/brief/en?WT.mc_id=exp).
 
-## Getting started
+Video link is [here](https://youtu.be/XoXDZBgbamc)
 
-The easiest way to get started with the controller is to run its Docker image from an Ubuntu Linux distribution:
+This version of LIPM walking controller is mainly developed by [Arnaud Tanguy](https://github.com/arntanguy).
 
-```
-xhost +local:docker
-docker run -it --rm --user ayumi -e DISPLAY=${DISPLAY} -v /tmp/.X11-unix:/tmp/.X11-unix:rw \
-    stephanecaron/lipm_walking_controller \
-    lipm_walking --floor
-```
+He fixed the "double stepping" problem, communication delay between several PCs for the experiment, and so on.
 
-See the [documentation](http://jrl-umi3218.github.io/lipm_walking_controller/doxygen/HEAD/docker.html) for more usage instructions.
 
-## Installation
+And then, [Masato Tsuru](https://github.com/TsuruMasato) added an interface for PS4 controller, Oculus controller, and RViz gui menu.
 
-The controller has been tested on Ubuntu 14.04, Ubuntu 16.04 and Ubuntu 18.04. See the instructions to [build and install](http://jrl-umi3218.github.io/lipm_walking_controller/doxygen/HEAD/build.html) in the documentation.
+If you want to try walking HRP4CR with PS4 Joystick, this version must be the easiest way.
 
-## Usage
 
-If it is not there already, enable the controller in your mc\_rtc configuration:
-```json
-{
-  "MainRobot": "JVRC1",
-  "Enabled": ["LIPMWalking"]
-}
-```
-Launch RViz for the JVRC-1 model by:
-```sh
-roslaunch mc_rtc_ticker display.launch
-```
-Finally, start the controller from your mc\_rtc interface. Here is the example
-of the [Choreonoid](https://choreonoid.org/en/) project installed from
-[mc\_openrtm](https://github.com/jrl-umi3218/mc_openrtm) in the
-Docker image:
-```sh
-cd /usr/share/hrpsys/samples/JVRC1
-choreonoid --start-simulation sim_mc.cnoid
+___
+## Required environments
+
+Required components and libraries :
+* Ubuntu 18.04 LTS
+* ROS melodic (desktop full)
+* mc_rtc, the latest version (2021/1/18) https://github.com/jrl-umi3218/mc_rtc
+* openrtp environment (private repository)
+* HRP4CR robot model and its mc_rtc support (private repository)
+* Online Footstep Planner, the latest version (2021/1/18) https://github.com/isri-aist/OnlineFootstepPlanner
+* PS4 controller and USB cable
+
+__Basically, drcutl's environment setup script and mc_rtc installation script automatically set up most of those libraries.__
+
+__Only "OnlineFootstepPlanner" is not automatically installed by those scripts.__
+
+About the installation of OnlineFootstepPlanner, please visit [its page](https://github.com/isri-aist/OnlineFootstepPlanner)
+
+___
+## How to build
+
+#### 1. download this repository: 
+
+``$ git clone [email protected]:TsuruMasato/lipm_walking_controller.git `` (default branch "rebase_stabilizer_ana")
+
+
+#### 2. go into the repository and make "build" directory: 
+
+``$ mkdir build``
+
+
+#### 3. go into the build directory and run ccmake :
+
+``$ ccmake ..``
+
+Turn on **AUTOLOAD_ExternalFootstepPlannerPlugin** option.
+
+And also, don't forget to set **CMake Install prefix** to /home/*your_name*/openrtp.
+
+
+#### 4. build this controller, and install it :
+
+``$ make -j8``
+
+``$ make install``
+
+
+___
+## How to run
+
+
+First, please switch the mc_controller in your mc\_rtc configuration as doing below :
+
+``$ nano ~/.config/mc_rtc/mc_rtc.yaml``
+
+```yaml
+MainRobot: HRP4CR
+Timestep: 0.002
+Enabled: LIPMWalking
 ```
-You should end up with the following windows:
-![Choreonoid and RViz GUI of the controller](https://user-images.githubusercontent.com/1189580/64157945-ead71c80-ce37-11e9-9081-7936702c5fbc.png)
-See the [Graphical user interface](https://github.com/stephane-caron/lipm_walking_controller/wiki/Graphical-user-interface) page of the
-wiki for further instructions on how to use this GUI.
 
-## Thanks
 
-To [@gergondet](https://github.com/gergondet) for developing and helping with the mc\_rtc framework.
+### You need at least these 4 terminal windows.
+
+* ROS core
+* Choreonoid simulation
+* RViz as mc_rtc control interface
+* Online Footstep Planner
+
+
+***
+
+### process
+
+
+#### 1. start ROS core in 1th terminal :
+
+``$ roscore ``
+
+
+#### 2. Please go to the HRP4CR directory in openrtp system in 2nd terminal :
+
+``$ cd ~/openrtp/share/hrpsys/samples/HRP4CR``
+
+
+#### 3. Start Choreonoid simulation in 2nd terminal :
+
+``$ choreonoid sim_mc_openrtp_bush.cnoid ``
+
+and please click the starting button in Choreonoid.
+
+Now, the LIPM walking controller is running.
+The HRP4CR robot model will keep standing.
+
+
+
+_(if the robot fails on the ground, `$ clear-omninames.sh` sometimes solve it.)_
+
+_(Even clear-omninames.sh cannot help you, your hrpsys-humanoid is too old. please update all libraries with drcutl script.)_
+
+
+![standing HRP4CR](https://github.com/TsuruMasato/lipm_walking_controller/blob/rebase_stabilizer_ana/image/Screenshot%20from%202022-01-18%2018-28-38.png)
+
+
+
+#### 4. Start RViz in 3rd terminal :
+
+`` $ roslaunch mc_rtc_ticker display.launch ``
+
+![RViz panel](https://github.com/TsuruMasato/lipm_walking_controller/blob/rebase_stabilizer_ana/image/Screenshot%20from%202022-01-18%2018-28-46.png)
+
+
+#### 5. Plug your PS4 controller to a USB port, and start Online Footstep Planner in 4th terminal :
+
+`` $ roslaunch online_footstep_planner run_demo_in_sim_rebase.launch ``
+
+Joystick connection node and Online Footstep Planner node start.
+
+If you can see the yellow warning message "received a map" like below-right side of the picture, it's success!!
+
+
+![terimal_message](https://github.com/TsuruMasato/lipm_walking_controller/blob/rebase_stabilizer_ana/image/Screenshot%20from%202022-01-18%2018-29-11.png)
+
+
+___
+
+___
+
+## GUI menu
+
+After starting the whole system, you have to select some GUI options.
+
+#### 1. Go to "Walking" tab in the control panel in RViz
+
+
+![Walking tab in RViz](https://github.com/TsuruMasato/lipm_walking_controller/blob/rebase_stabilizer_ana/image/Screenshot%20from%202022-01-18%2018-40-29.png)
+
+
+#### 2. Click "start standing" button.
+
+_The stabilizer task becomes enable, and HRP4CR starts balancing._
+
+
+#### 3. Set Footstep plan menu to "external"
+
+#### 4. Change Target type to "PS4 Controller"
+
+![Final state of RViz panel](https://github.com/TsuruMasato/lipm_walking_controller/blob/rebase_stabilizer_ana/image/Screenshot%20from%202022-01-18%2018-41-16.png)
+
+
+#### 5. Now you can walk the robot via Joysticks!!
+
+
+##### Left Joystick leads the robot walking foward/backward/left/right, and Right Joystick makes him turn.
+
+
diff --git a/etc/LIPMWalking.in.yaml b/etc/LIPMWalking.in.yaml
@@ -56,7 +56,7 @@ robot_models:
       dcm_tracking:
         gains:
           prop: 4
-          integral: 0
+          integral: 5
           deriv: 0.2
       admittance:
         cop: [0.03, 0.04]
@@ -67,15 +67,16 @@ robot_models:
         contact:
           damping: 300.
       dcm_bias:
-        #biasDriftPerSeconStd: 0.0001
+        dcmMeasureErrorStd: 0.001
+        biasDriftPerSeconStd: 0.0001
         zmpMeasureErrorStd: 0.0005
-        biasLimit: [0.05, 0.05]
+        biasLimit: [0.05, 0.01]
         withDCMBias: true
     sole:
       half_length: 0.08
       half_width: 0.038
       friction: 0.7
-      leftAnkleOffset: [-0.05, 0.] # target center of foot
+      leftAnkleOffset: [-0.03, -0.02] # target center of foot
     torso:
       pitch: 0.0
   hrp5_p:
@@ -654,6 +655,11 @@ plans:
         - pose:
             translation: [0.035, 0.07, 0.0]
           surface: LeftFootCenter
+      mpc:
+        weights:
+          jerk: 1.0
+          vel: [10.0, 100.0]
+          zmp: 1000.0
       # Offset (x,y,theta) between the plan's target frame and the left/right foot expressed in target frame
       leftFootLandingOffset: [0.0, 0.07, 0.0] # x, y, theta
       rightFootLandingOffset: [0.0, -0.07, 0.0] # x, y, theta

diff --git a/image/Screenshot from 2022-01-18 18-28-38.png b/image/Screenshot from 2022-01-18 18-28-38.png
diff --git a/image/Screenshot from 2022-01-18 18-28-46.png b/image/Screenshot from 2022-01-18 18-28-46.png
diff --git a/image/Screenshot from 2022-01-18 18-29-11.png b/image/Screenshot from 2022-01-18 18-29-11.png
diff --git a/image/Screenshot from 2022-01-18 18-40-29.png b/image/Screenshot from 2022-01-18 18-40-29.png
diff --git a/image/Screenshot from 2022-01-18 18-41-16.png b/image/Screenshot from 2022-01-18 18-41-16.png
diff --git a/image/Screenshot from 2022-01-18 19-17-45.png b/image/Screenshot from 2022-01-18 19-17-45.png
diff --git a/launch/run_online_footstep_planner_and_joy_node.launch b/launch/run_online_footstep_planner_and_joy_node.launch
@@ -0,0 +1,16 @@
+<launch>
+
+<!-- input converter -->
+<arg name="verbose" default="false" />
+
+<!-- PS4 controller -->
+<node name="joy_node" pkg="joy" type="joy_node" >
+    <remap from="joy" to="/avatar/joy" />
+</node>
+
+<!-- footstep planner -->
+<include file="$(find online_footstep_planner)/launch/planner.launch">
+    <arg name="robot" value="hrp4cr" />
+</include>
+
+</launch>
diff --git a/plugins/ExternalFootstepPlanner/CMakeLists.txt b/plugins/ExternalFootstepPlanner/CMakeLists.txt
@@ -32,12 +32,14 @@ set(ExternalFootstepPlanner_SRC
   src/ExternalFootstepPlannerPlugin.cpp
   src/ExternalFootstepPlanner.cpp
   src/OnlineFootstepPlanner.cpp
+  src/InputConvertor.cpp
 )
 
 set(ExternalFootstepPlanner_HDR
   include/ExternalFootstepPlanner/ExternalFootstepPlannerPlugin.h
   include/ExternalFootstepPlanner/ExternalFootstepPlanner.h
   include/ExternalFootstepPlanner/OnlineFootstepPlanner.h
+  include/ExternalFootstepPlanner/InputConvertor.h
 )
 
 

diff --git a/plugins/ExternalFootstepPlanner/etc/ExternalFootstepPlannerPlugin.yaml b/plugins/ExternalFootstepPlanner/etc/ExternalFootstepPlannerPlugin.yaml
@@ -1,4 +1,7 @@
 category: ["Walking", "Main", "External Planning"]
+
+default_target_type: "Oculus Controller"
+
 velocity_target:
   planning_distance: [0.15, 0.15, 1.57]
 

diff --git a/...s/ExternalFootstepPlanner/include/ExternalFootstepPlanner/ExternalFootstepPlannerPlugin.h b/...s/ExternalFootstepPlanner/include/ExternalFootstepPlanner/ExternalFootstepPlannerPlugin.h
@@ -6,8 +6,11 @@
 
 #include <mc_control/GlobalPlugin.h>
 
+#include <sensor_msgs/Joy.h>
+
 #include <ExternalFootstepPlanner/ExternalFootstepPlanner.h>
 #include <ExternalFootstepPlanner/ExternalFootstepPlannerPlugin.h>
+#include <ExternalFootstepPlanner/InputConvertor.h>
 #include <ExternalFootstepPlanner/SE2d.h>
 
 namespace mc_plugin
@@ -106,6 +109,20 @@ struct ExternalFootstepPlannerPlugin : public mc_control::GlobalPlugin
    */
   void setLocalVelocityPlanningDistance(const SE2d & distance);
 
+  /**
+   * @brief [Tsuru add] Sets the Local Velocity Target object by ROS Joystick input
+   * @note For now this is implemented as requesting a local target as none of the supported planners support velocity
+   * inputs. Also the planner does not have suitable input parameters to match the footstep length to the desired
+   * velocity. As a result "velocity" here is interpreted in an arbitrary unity and should be seen as a desired
+   * direction.
+   *
+   * @note Historical background: this is a direct replacement for the deprecated input_convertor_node
+   *
+   * @param joystickInput ROS message for biped walking in local frame
+   * @see setLocalPositionTarget
+   */
+  void setJoystickVelocityTarget(const sensor_msgs::Joy & joystickInput);
+
   /**
    * @brief Add GUI elements that are visible when the planner is available
    */
@@ -137,7 +154,8 @@ struct ExternalFootstepPlannerPlugin : public mc_control::GlobalPlugin
   std::unique_ptr<ExternalFootstepPlanner> planner_{nullptr}; ///< Planner implementation
   bool wasAvailable_ = false; ///< True if the planner was active during the previous iteration
 
-  std::vector<std::string> supportedTargetTypes_{"World SE2", "Local SE2", "Local Velocity"};
+  std::vector<std::string> supportedTargetTypes_{"World SE2", "Local SE2", "Local Velocity", "PS4 Controller",
+                                                 "Oculus Controller"};
   std::string targetType_{"World SE2"};
 
   bool worldPositionTargetChanged_ =
@@ -149,6 +167,18 @@ struct ExternalFootstepPlannerPlugin : public mc_control::GlobalPlugin
 
   SE2d localVelocityTarget_{};
   SE2d planningDistance_{0.3, 0.3, mc_rtc::constants::PI / 2}; // How far ahead should we plan?
+
+  /* Tsuru add */
+  std::thread joystickSubscribeThread_;
+
+protected:
+  void joystickSubscribeThread();
+  void joystick_callback(const sensor_msgs::JoyConstPtr & joystick_input);
+  std::string joystick_topic_ = "/avatar/joy";
+  double rate_ = 30;
+  bool run_{false};
+  bool joystickPositionTargetChanged_{false};
+  bool isControllerConnected_{false};
 };
 
 } // namespace ExternalFootstepPlanner

diff --git a/plugins/ExternalFootstepPlanner/include/ExternalFootstepPlanner/InputConvertor.h b/plugins/ExternalFootstepPlanner/include/ExternalFootstepPlanner/InputConvertor.h
@@ -0,0 +1,35 @@
+#pragma once
+
+#include <geometry_msgs/Pose2D.h>
+#include <ros/ros.h>
+#include <sensor_msgs/Joy.h>
+
+#include <ExternalFootstepPlanner/SE2d.h>
+#include <atomic>
+#include <map>
+#include <math.h>
+#include <mutex>
+#include <random>
+// #include <thread>
+#include <vector>
+
+namespace mc_plugin
+{
+namespace ExternalFootstepPlanner
+{
+
+class InputConvertor
+{
+public:
+  InputConvertor() = delete;
+  ~InputConvertor() = delete;
+
+  static SE2d convert_PS4_to_SE2d(const sensor_msgs::Joy & joy_msg);
+  static SE2d convert_Oculus_to_SE2d(const sensor_msgs::Joy & joy_msg);
+  static double L2norm(double a_x, double a_y, double b_x = 0.0, double b_y = 0.0); // calc L2 distance between A and B
+  static double Uniform_norm(double a_x, double a_y);
+  static double calc_exponential_interpolation(double exp, double y_max, double x);
+};
+
+} // namespace ExternalFootstepPlanner
+} // namespace mc_plugin