Merge pull request #100 from robotology/devel

Merge devel into master

Author: GiulioRomualdi

CHANGELOG.md

@@ -3,8 +3,18 @@ All notable changes to this project are documented in this file.
 
 ## [Unreleased]
 ### Added
+- Add the possibility to use the walking-module with iCubGenova09 (https://github.com/robotology/walking-controllers/pull/80)
+- Add the possibility to use the `FreeSpaceEllipse` in the `TrajectoryPlanner` (https://github.com/robotology/walking-controllers/pull/82)
+- Enable the `iFeel` retargeting pipeline (https://github.com/robotology/walking-controllers/pull/87)
+- Bipedal locomotion v0.6.0 is now required to compile the project (https://github.com/robotology/walking-controllers/pull/97)
+- Add the possibility to control the root link height instead of the CoM height (https://github.com/robotology/walking-controllers/pull/93 and https://github.com/robotology/walking-controllers/pull/96)
 
 ### Changed
+- Add the possibility to handle multiple wrenches in the RobotInterface/Helper class (https://github.com/robotology/walking-controllers/pull/80)
+- Using the `feetYawDelta` and the `slowWhenBackwardFactor` in the `TrajectoryPlanner` (https://github.com/robotology/walking-controllers/pull/89)
+- `WalkingModule` logs the data through bipedal locomotion framework logger (https://github.com/robotology/walking-controllers/pull/97)
+
+### Fixed
 - Fixed missing link library in `WholeBodyControllers` component  (https://github.com/robotology/walking-controllers/pull/81).
 
 ## [0.4.1] - 2020-02-04
@@ -121,7 +131,8 @@ All notable changes to this project are documented in this file.
 - Implement the first version of the `WalkingLoggerModule`
 - Implement the first version of the `WalkingJoypadModule`
 
-[Unreleased]: https://github.com/robotology/walking-controllers/compare/v0.4.0...devel
+[Unreleased]: https://github.com/robotology/walking-controllers/compare/v0.4.1...devel
+[0.4.1]: https://github.com/robotology/walking-controllers/compare/v0.4.0...v0.4.1
 [0.4.0]: https://github.com/robotology/walking-controllers/compare/v0.3.3...v0.4.0
 [0.3.3]: https://github.com/robotology/walking-controllers/compare/v0.3.2...v0.3.3
 [0.3.2]: https://github.com/robotology/walking-controllers/compare/v0.3.1...v0.3.2

README.md

@@ -87,6 +87,7 @@ export YARP_DATA_DIRS=$YARP_DATA_DIRS:$WalkingControllers_INSTALL_DIR/share/yarp
 3. Run gazebo and drag and drop iCub (e.g. icubGazeboSim or iCubGazeboV2_5):
 
     ``` sh
+    export YARP_CLOCK=/clock
     gazebo -slibgazebo_yarp_clock.so
     ```
 4. Run `yarprobotinterface`

cmake/WalkingControllersFindDependencies.cmake

@@ -134,7 +134,7 @@ checkandset_dependency(iDynTree)
 find_package(Eigen3 3.2.92 QUIET)
 checkandset_dependency(Eigen3)
 
-find_package(UnicyclePlanner 0.1.102 QUIET)
+find_package(UnicyclePlanner 0.4.0 QUIET)
 checkandset_dependency(UnicyclePlanner)
 
 find_package(osqp QUIET)
@@ -149,6 +149,8 @@ checkandset_dependency(qpOASES)
 find_package(Catch2 QUIET)
 checkandset_dependency(Catch2)
 
+find_package(BipedalLocomotionFramework 0.6.0 COMPONENTS VectorsCollection REQUIRED)
+
 walking_controllers_dependent_option(WALKING_CONTROLLERS_COMPILE_tests "Compile tests?" ON WALKING_CONTROLLERS_HAS_Catch2 OFF)
 walking_controllers_dependent_option(WALKING_CONTROLLERS_COMPILE_YarpUtilities "Compile YarpHelper library?" ON WALKING_CONTROLLERS_HAS_YARP OFF)
 walking_controllers_dependent_option(WALKING_CONTROLLERS_COMPILE_iDynTreeUtilities "Compile iDynTreeHelper library?" ON "WALKING_CONTROLLERS_HAS_iDynTree;WALKING_CONTROLLERS_HAS_YARP;WALKING_CONTROLLERS_HAS_Eigen3" OFF)

src/JoypadModule/app/dcmWalkingJoypad/joypad.ini

@@ -17,8 +17,8 @@ deadzone        0.05
 fullscale       1.0
 
 # scale factors
-scale_x         5.0
-scale_y         5.0
+scale_x         10.0
+scale_y         10.0
 
 # Port names
 rpcServerPort_name              /walking-coordinator/rpc

src/KinDynWrapper/include/WalkingControllers/KinDynWrapper/Wrapper.h

@@ -261,6 +261,7 @@ namespace WalkingControllers
          */
         bool getCoMJacobian(iDynTree::MatrixDynSize &jacobian);
 
+        bool getRootLinkJacobian(iDynTree::MatrixDynSize &jacobian);
         /**
          * Get the joint position
          * @return the joint position expressed in radians

src/KinDynWrapper/src/Wrapper.cpp

@@ -512,6 +512,11 @@ bool WalkingFK::getLeftHandJacobian(iDynTree::MatrixDynSize &jacobian)
     return m_kinDyn.getFrameFreeFloatingJacobian(m_frameLeftHandIndex, jacobian);
 }
 
+bool WalkingFK::getRootLinkJacobian(iDynTree::MatrixDynSize &jacobian)
+{
+    return m_kinDyn.getFrameFreeFloatingJacobian(m_frameRootIndex, jacobian);
+}
+
 bool WalkingFK::getNeckJacobian(iDynTree::MatrixDynSize &jacobian)
 {
     return m_kinDyn.getFrameFreeFloatingJacobian(m_frameNeckIndex, jacobian);

src/RobotInterface/include/WalkingControllers/RobotInterface/Helper.h

@@ -77,17 +77,21 @@ namespace WalkingControllers
         std::unique_ptr<iCub::ctrl::FirstOrderLowPassFilter> m_velocityFilter; /**< Joint velocity low pass filter .*/
         bool m_useVelocityFilter; /**< True if the joint velocity filter is used. */
 
-        yarp::os::BufferedPort<yarp::sig::Vector> m_leftWrenchPort; /**< Left foot wrench port. */
-        yarp::os::BufferedPort<yarp::sig::Vector> m_rightWrenchPort; /**< Right foot wrench port. */
-        yarp::sig::Vector m_leftWrenchInput; /**< YARP vector that contains left foot wrench. */
-        yarp::sig::Vector m_rightWrenchInput; /**< YARP vector that contains right foot wrench. */
-        yarp::sig::Vector m_leftWrenchInputFiltered; /**< YARP vector that contains left foot filtered wrench. */
-        yarp::sig::Vector m_rightWrenchInputFiltered; /**< YARP vector that contains right foot filtered wrench. */
+        struct MeasuredWrench
+        {
+            std::unique_ptr<yarp::os::BufferedPort<yarp::sig::Vector>> port; /**< yarp port. */
+            yarp::sig::Vector wrenchInput; /**< YARP vector that contains foot wrench. */
+            yarp::sig::Vector wrenchInputFiltered; /**< YARP vector that contains foot filtered wrench. */
+            std::unique_ptr<iCub::ctrl::FirstOrderLowPassFilter> lowPassFilter; /**< Low pass filter.*/
+            bool useFilter;
+            bool isUpdated;
+        };
+
+        std::vector<MeasuredWrench> m_leftFootMeasuredWrench;
+        std::vector<MeasuredWrench> m_rightFootMeasuredWrench;
+
         iDynTree::Wrench m_leftWrench; /**< iDynTree vector that contains left foot wrench. */
         iDynTree::Wrench m_rightWrench; /**< iDynTree vector that contains right foot wrench. */
-        std::unique_ptr<iCub::ctrl::FirstOrderLowPassFilter> m_leftWrenchFilter; /**< Left wrench low pass filter.*/
-        std::unique_ptr<iCub::ctrl::FirstOrderLowPassFilter> m_rightWrenchFilter; /**< Right wrench low pass filter.*/
-        bool m_useWrenchFilter; /**< True if the wrench filter is used. */
 
         double m_startingPositionControlTime;
         bool m_positionMoveSkipped;
@@ -120,6 +124,14 @@ namespace WalkingControllers
         bool setInteractionMode(yarp::dev::InteractionModeEnum interactionMode);
 
         bool setInteractionMode(std::vector<yarp::dev::InteractionModeEnum>& interactionModes);
+
+        bool configureForceTorqueSensor(const std::string& portPrefix,
+                                        const std::string& portInputName,
+                                        const std::string& wholeBodyDynamicsPortName,
+                                        const double& samplingTime,
+                                        bool useWrenchFilter,
+                                        double cutFrequency,
+                                        MeasuredWrench& measuredWrench);
     public:
 
         /**