Merge pull request #642 from robotology/devel

Merge devel in master
robotology · Jan 13, 2023 · 94bbe6e · 94bbe6e
2 parents b9c18f4 + fe7a39e
commit 94bbe6e
Show file tree

Hide file tree

Showing 6 changed files with 433 additions and 99 deletions.
diff --git a/.github/workflows/conda-ci.yml b/.github/workflows/conda-ci.yml
@@ -23,9 +23,8 @@ jobs:
 
     - uses: conda-incubator/setup-miniconda@v2
       with:
-        mamba-version: "*"
-        channels: conda-forge,robotology
-        channel-priority: true
+        miniforge-variant: Mambaforge
+        miniforge-version: latest
 
     - uses: rlespinasse/[email protected]
 
@@ -37,7 +36,7 @@ jobs:
         # Compilation related dependencies
         mamba install cmake compilers make ninja pkg-config
         # Actual dependencies
-        mamba install -c conda-forge -c robotology yarp gazebo 
+        mamba install -c conda-forge yarp gazebo 
 
     # Additional dependencies useful only on Linux
     - name: Dependencies [Conda/Linux]

diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -5,6 +5,12 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 
 ## [Unreleased]
 
+## [4.6.0] - 2023-01-13
+
+### Added
+
+- In `gazebo_yarp_controlboard` add the `icub_hand_mk5` coupling that models the iCub mk5 hand, that is used also in ergoCub 1 robot (https://github.com/robotology/gazebo-yarp-plugins/pull/641).
+
 ## [4.5.2] - 2022-11-17
 
 ### Fixed
@@ -20,7 +26,7 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 ### Added
 - The `gazebo_yarp_forcetorque` plugin now exposes also the `yarp::dev::ISixAxisForceTorqueSensors` interface, so it can be used with ` multipleanalogsensorsserver`, `multipleanalogsensorsremapper` and `multipleanalogsensorsclient` devices (https://github.com/robotology/gazebo-yarp-plugins/issues/384, https://github.com/robotology/gazebo-yarp-plugins/pull/628).
 
-### Fixed 
+### Fixed
 - Fix YARP 3.8 compatibility (https://github.com/robotology/gazebo-yarp-plugins/pull/627).
 
 ## [4.4.0] - 2022-05-31
@@ -33,12 +39,12 @@ The format of this document is based on [Keep a Changelog](https://keepachangelo
 
 ### Added
 - In `gazebo_yarp_camera` parse the `yarpDeviceName` option to enable its use with `gazebo_yarp_robotinterface` (https://github.com/robotology/gazebo-yarp-plugins/pull/614).
-- In `gazebo_yarp_controlboard` add the `icub_left_hand_mk4` coupling that models the iCub mk4 left hand (https://github.com/robotology/gazebo-yarp-plugins/pull/620). 
+- In `gazebo_yarp_controlboard` add the `icub_left_hand_mk4` coupling that models the iCub mk4 left hand (https://github.com/robotology/gazebo-yarp-plugins/pull/620).
 
 ### Changed
-- Migrate the example models under the tutorial directory to avoid the use of implicit network wrapper servers, and use the `gazebo_yarp_robotinterface` plugin to spawn their network wrapper servers (https://github.com/robotology/gazebo-yarp-plugins/pull/615 and https://github.com/robotology/gazebo-yarp-plugins/pull/616). 
+- Migrate the example models under the tutorial directory to avoid the use of implicit network wrapper servers, and use the `gazebo_yarp_robotinterface` plugin to spawn their network wrapper servers (https://github.com/robotology/gazebo-yarp-plugins/pull/615 and https://github.com/robotology/gazebo-yarp-plugins/pull/616).
 
-### Fixed 
+### Fixed
 - It is now possible to remove and add again to the simulation models that use `gazebo_yarp_robotinterface` without any crash (https://github.com/robotology/gazebo-yarp-plugins/pull/618, https://github.com/robotology/gazebo-yarp-plugins/pull/619).
 - Fixed value returned by getDeviceStatus method in `gazebo_yarp_laser` plugin (https://github.com/robotology/gazebo-yarp-plugins/pull/617).
 
@@ -56,12 +62,12 @@ disable any network wrapper server that the plugin created, by just creating the
 
 ## [4.1.2] - 2022-01-19
 
-### Fixed 
+### Fixed
 - Fix compilation against YARP 3.7 (https://github.com/robotology/gazebo-yarp-plugins/pull/607, https://github.com/robotology/gazebo-yarp-plugins/issues/608).
 
 ## [4.1.1] - 2022-01-13
 
-### Fixed 
+### Fixed
 - Fix compilation against Gazebo 11.10.0 (https://github.com/robotology/gazebo-yarp-plugins/pull/605, https://github.com/robotology/gazebo-yarp-plugins/issues/606).
 
 ## [4.1.0] - 2021-12-23

diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -8,8 +8,8 @@ option(GAZEBO_YARP_PLUGINS_DISABLE_IMPLICIT_NETWORK_WRAPPERS "if enabled removes
 
 # Project version
 set(${PROJECT_NAME}_MAJOR_VERSION 4)
-set(${PROJECT_NAME}_MINOR_VERSION 5)
-set(${PROJECT_NAME}_PATCH_VERSION 2)
+set(${PROJECT_NAME}_MINOR_VERSION 6)
+set(${PROJECT_NAME}_PATCH_VERSION 0)
 
 set(${PROJECT_NAME}_VERSION
     ${${PROJECT_NAME}_MAJOR_VERSION}.${${PROJECT_NAME}_MINOR_VERSION}.${${PROJECT_NAME}_PATCH_VERSION})

diff --git a/plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h b/plugins/controlboard/include/yarp/dev/ControlBoardDriverCoupling.h
@@ -187,7 +187,7 @@ class HandMk3CouplingHandler : public BaseCouplingHandler
     yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref);
     yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref);
     yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
-    
+
 protected:
     double decouple (double q2, std::vector<double>& lut);
 
@@ -212,7 +212,33 @@ class HandMk4CouplingHandler : public BaseCouplingHandler
     yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref);
     yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref);
     yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
-
+
+protected:
+    double decouple (double q2, std::vector<double>& lut);
+
+    const int LUTSIZE;
+
+    std::vector<double> thumb_lut;
+    std::vector<double> index_lut;
+    std::vector<double> pinkie_lut;
+};
+
+class HandMk5CouplingHandler : public BaseCouplingHandler
+{
+
+public:
+    HandMk5CouplingHandler (gazebo::physics::Model* model, yarp::sig::VectorOf<int> coupled_joints, std::vector<std::string> coupled_joint_names, std::vector<Range> coupled_joint_limits);
+
+public:
+    bool decouplePos (yarp::sig::Vector& current_pos);
+    bool decoupleVel (yarp::sig::Vector& current_vel);
+    bool decoupleAcc (yarp::sig::Vector& current_acc);
+    bool decoupleTrq (yarp::sig::Vector& current_trq);
+
+    yarp::sig::Vector decoupleRefPos (yarp::sig::Vector& pos_ref);
+    yarp::sig::Vector decoupleRefVel (yarp::sig::Vector& vel_ref);
+    yarp::sig::Vector decoupleRefTrq (yarp::sig::Vector& trq_ref);
+
 protected:
     double decouple (double q2, std::vector<double>& lut);
 

diff --git a/plugins/controlboard/src/ControlBoardDriver.cpp b/plugins/controlboard/src/ControlBoardDriver.cpp
@@ -337,6 +337,12 @@ bool GazeboYarpControlBoardDriver::gazebo_init()
                 m_coupling_handler.push_back(cpl);
                 yCInfo(GAZEBOCONTROLBOARD) << "using icub_left_hand_mk4";
             }
+            else if (coupling_bottle->get(0).asString()=="icub_hand_mk5")
+            {
+                BaseCouplingHandler* cpl = new HandMk5CouplingHandler(m_robot,coupled_joints, coupled_joint_names, coupled_joint_limits);
+                m_coupling_handler.push_back(cpl);
+                yCInfo(GAZEBOCONTROLBOARD) << "using icub_hand_mk5";
+            }
             else if (coupling_bottle->get(0).asString()=="none")
             {
                 yCDebug(GAZEBOCONTROLBOARD) << "Just for test";