├── src
    ├── states
    │   ├── data
    │   │   └── states.json
    │   ├── CMakeLists.txt
    │   ├── InitialPosture.h
    │   ├── CheckResults.h
    │   ├── CalibrationMotion.h
    │   ├── PressureCheck.h
    │   ├── ChooseTransition.h
    │   ├── RunCalibrationScript.h
    │   ├── CalibrationMotionLogging.h
    │   ├── ChooseTransition.cpp
    │   ├── InitialPosture.cpp
    │   ├── PressureCheck.cpp
    │   ├── ShowForces.h
    │   ├── CalibrationMotionLogging.cpp
    │   ├── CalibrationMotion.cpp
    │   ├── CheckResults.cpp
    │   ├── RunCalibrationScript.cpp
    │   └── ShowForces.cpp
    ├── lib.cpp
    ├── Measurement.h
    ├── ForceSensorCalibration.h
    ├── CMakeLists.txt
    ├── api.h
    ├── ForceSensorCalibration.cpp
    ├── calibrate.h
    └── calibrate.cpp
├── .gitignore
├── doc
    └── hrp4_calibration_example.png
├── .clang-format-fix.sh
├── .clang-format-check.sh
├── .clang-format-common.sh
├── CMakeLists.txt
├── LICENSE
├── .github
    └── workflows
    │   └── build.yml
├── .clang-format
├── etc
    └── ForceSensorCalibration.in.yaml
└── README.md


/src/states/data/states.json:
--------------------------------------------------------------------------------
1 | {
2 | }


--------------------------------------------------------------------------------
/.gitignore:
--------------------------------------------------------------------------------
1 | build/
2 | compile_commands.json
3 | 


--------------------------------------------------------------------------------
/src/lib.cpp:
--------------------------------------------------------------------------------
1 | #include "ForceSensorCalibration.h"
2 | 
3 | CONTROLLER_CONSTRUCTOR("ForceSensorCalibration", ForceSensorCalibration)
4 | 


--------------------------------------------------------------------------------
/doc/hrp4_calibration_example.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/jrl-umi3218/mc_force_sensor_calibration_controller/HEAD/doc/hrp4_calibration_example.png


--------------------------------------------------------------------------------
/.clang-format-fix.sh:
--------------------------------------------------------------------------------
 1 | #!/bin/bash
 2 | 
 3 | readonly this_dir=`cd $(dirname $0); pwd`
 4 | cd $this_dir
 5 | source .clang-format-common.sh
 6 | 
 7 | for f in ${src_files}; do
 8 |   $clang_format -style=file -i $f
 9 | done
10 | 


--------------------------------------------------------------------------------
/src/states/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | add_fsm_state_simple(PressureCheck)
 2 | add_fsm_state_simple(ChooseTransition)
 3 | add_fsm_state_simple(ShowForces)
 4 | add_fsm_state_simple(InitialPosture)
 5 | add_fsm_state_simple(CalibrationMotion)
 6 | add_fsm_state_simple(CalibrationMotionLogging)
 7 | add_fsm_state_simple(RunCalibrationScript)
 8 | add_fsm_state_simple(CheckResults)
 9 | 
10 | add_fsm_data_directory(data)
11 | 
12 | target_include_directories(RunCalibrationScript PRIVATE "${PROJECT_BINARY_DIR}/src")
13 | 


--------------------------------------------------------------------------------
/src/Measurement.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <SpaceVecAlg/SpaceVecAlg>
 4 | 
 5 | #include <map>
 6 | #include <string>
 7 | #include <vector>
 8 | 
 9 | /** A measurement for the calibration */
10 | struct Measurement
11 | {
12 |   /** Position of the force sensor's parent body */
13 |   sva::PTransformd X_0_p;
14 |   /** Raw force sensor reading */
15 |   sva::ForceVecd measure;
16 | };
17 | 
18 | using Measurements = std::vector<Measurement>;
19 | using SensorMeasurements = std::map<std::string, Measurements>;
20 | 


--------------------------------------------------------------------------------
/.clang-format-check.sh:
--------------------------------------------------------------------------------
 1 | #!/bin/bash
 2 | 
 3 | readonly this_dir=`cd $(dirname $0); pwd`
 4 | cd $this_dir
 5 | source .clang-format-common.sh
 6 | 
 7 | out=0
 8 | tmpfile=$(mktemp /tmp/clang-format-check.XXXXXX)
 9 | for f in ${src_files}; do
10 |   $clang_format -style=file $f > $tmpfile
11 |   if ! [[ -z `diff $tmpfile $f` ]]; then
12 |     echo "Wrong formatting in $f"
13 |     out=1
14 |   fi
15 | done
16 | rm -f $tmpfile
17 | if [[ $out -eq 1 ]]; then
18 |   echo "You can run ./.clang-format-fix.sh to fix the issues locally, then commit/push again"
19 | fi
20 | exit $out
21 | 


--------------------------------------------------------------------------------
/.clang-format-common.sh:
--------------------------------------------------------------------------------
 1 | # This script is meant to be sourced from other scripts
 2 | 
 3 | # Check for clang-format, prefer 6.0 if available
 4 | if [[ -x "$(command -v clang-format-6.0)" ]]; then
 5 |   clang_format=clang-format-6.0
 6 | elif [[ -x "$(command -v clang-format)" ]]; then
 7 |   clang_format=clang-format
 8 | else
 9 |   echo "clang-format or clang-format-6.0 must be installed"
10 |   exit 1
11 | fi
12 | 
13 | # Find all source files in the project minus those that are auto-generated or we do not maintain
14 | src_files=`find src -name '*.cpp' -or -name '*.h' -or -name '*.hpp'`
15 | 


--------------------------------------------------------------------------------
/src/states/InitialPosture.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <mc_control/CompletionCriteria.h>
 4 | #include <mc_control/fsm/State.h>
 5 | #include <fstream>
 6 | 
 7 | struct InitialPosture : mc_control::fsm::State
 8 | {
 9 | 
10 |   void configure(const mc_rtc::Configuration & config) override {}
11 | 
12 |   void start(mc_control::fsm::Controller & ctl) override;
13 | 
14 |   bool run(mc_control::fsm::Controller & ctl) override;
15 | 
16 |   inline void teardown(mc_control::fsm::Controller & ctl) override {}
17 | 
18 | private:
19 |   double savedStiffness_ = 1;
20 |   mc_control::CompletionCriteria crit_;
21 | };
22 | 


--------------------------------------------------------------------------------
/src/states/CheckResults.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <mc_control/fsm/State.h>
 4 | 
 5 | struct CheckResults : mc_control::fsm::State
 6 | {
 7 | 
 8 |   void configure(const mc_rtc::Configuration & config) override;
 9 | 
10 |   void start(mc_control::fsm::Controller & ctl) override;
11 | 
12 |   bool run(mc_control::fsm::Controller & ctl) override;
13 | 
14 |   void teardown(mc_control::fsm::Controller & ctl) override;
15 | 
16 |   void saveCalibration(mc_control::fsm::Controller & ctl);
17 | 
18 | private:
19 |   double t_ = 0;
20 |   std::vector<std::string> sensors_;
21 |   bool running_ = true;
22 |   bool checkDefault_ = false;
23 | };
24 | 


--------------------------------------------------------------------------------
/src/states/CalibrationMotion.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <mc_control/fsm/State.h>
 4 | 
 5 | struct CalibrationMotion : mc_control::fsm::State
 6 | {
 7 | 
 8 |   inline void configure(const mc_rtc::Configuration & config) override {}
 9 | 
10 |   void start(mc_control::fsm::Controller & ctl) override;
11 | 
12 |   bool run(mc_control::fsm::Controller & ctl) override;
13 | 
14 |   void teardown(mc_control::fsm::Controller & ctl) override;
15 | 
16 | private:
17 |   std::vector<std::function<void()>> jointUpdates_;
18 |   double dt_ = 0;
19 |   double duration_ = 60;
20 |   double percentLimits_ = 0.8;
21 |   double savedStiffness_ = 10;
22 |   bool interrupted_ = false;
23 | };
24 | 


--------------------------------------------------------------------------------
/src/states/PressureCheck.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <mc_control/CompletionCriteria.h>
 4 | #include <mc_control/fsm/State.h>
 5 | #include <fstream>
 6 | 
 7 | struct PressureCheck : mc_control::fsm::State
 8 | {
 9 |   void configure(const mc_rtc::Configuration & config) override {}
10 |   void start(mc_control::fsm::Controller & ctl) override;
11 |   void teardown(mc_control::fsm::Controller & ctl) override;
12 |   bool run(mc_control::fsm::Controller & ctl) override;
13 | 
14 |   void check(mc_control::fsm::Controller & ctl);
15 | 
16 | protected:
17 |   bool success_ = false;
18 |   double maxPressure_ = 50;
19 |   std::vector<std::pair<std::string, std::string>> forceSensors_;
20 | };
21 | 


--------------------------------------------------------------------------------
/src/states/ChooseTransition.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <mc_control/fsm/State.h>
 4 | 
 5 | namespace mc_control
 6 | {
 7 | namespace fsm
 8 | {
 9 | struct Controller;
10 | } // namespace fsm
11 | } // namespace mc_control
12 | 
13 | struct ChooseTransition : mc_control::fsm::State
14 | {
15 |   void configure(const mc_rtc::Configuration & config) override;
16 | 
17 |   void start(mc_control::fsm::Controller & ctl) override;
18 | 
19 |   bool run(mc_control::fsm::Controller & ctl) override;
20 | 
21 |   void teardown(mc_control::fsm::Controller & ctl) override;
22 | 
23 | private:
24 |   std::map<std::string, std::string> actions_;
25 |   std::vector<std::string> category_ = {"ChooseTransition"};
26 | };
27 | 


--------------------------------------------------------------------------------
/src/states/RunCalibrationScript.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <mc_control/fsm/State.h>
 4 | #include <atomic>
 5 | #include <fstream>
 6 | #include <thread>
 7 | 
 8 | struct RunCalibrationScript : mc_control::fsm::State
 9 | {
10 | 
11 |   ~RunCalibrationScript() override;
12 | 
13 |   void configure(const mc_rtc::Configuration & config) override;
14 | 
15 |   void start(mc_control::fsm::Controller & ctl) override;
16 | 
17 |   bool run(mc_control::fsm::Controller & ctl) override;
18 | 
19 |   void teardown(mc_control::fsm::Controller & ctl) override;
20 | 
21 | private:
22 |   std::thread th_;
23 |   std::atomic<bool> completed_{false};
24 |   bool success_ = true;
25 |   std::string outputPath_;
26 |   std::vector<std::string> sensors_;
27 | };
28 | 


--------------------------------------------------------------------------------
/src/ForceSensorCalibration.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <mc_control/fsm/Controller.h>
 4 | #include <mc_control/mc_controller.h>
 5 | 
 6 | #include "api.h"
 7 | 
 8 | struct ForceSensorCalibration_DLLAPI ForceSensorCalibration : public mc_control::fsm::Controller
 9 | {
10 |   ForceSensorCalibration(mc_rbdyn::RobotModulePtr rm, double dt, const mc_rtc::Configuration & config);
11 | 
12 |   bool run() override;
13 | 
14 |   void reset(const mc_control::ControllerResetData & reset_data) override;
15 | 
16 |   /**
17 |    * Do not create observers from global configuration.
18 |    * They will be created in the constructor according to the per-robot
19 |    * configuration instead
20 |    */
21 |   void createObserverPipelines(const mc_rtc::Configuration & config) override {}
22 | 
23 | private:
24 |   mc_rtc::Configuration config_;
25 | };
26 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.1)
 2 | 
 3 | # Required for Ceres
 4 | set(CMAKE_CXX_STANDARD 14)
 5 | 
 6 | set(PROJECT_NAME ForceSensorCalibration)
 7 | set(PROJECT_DESCRIPTION "ForceSensorCalibration")
 8 | set(PROJECT_URL "")
 9 | 
10 | project(${PROJECT_NAME} VERSION 1.0.0 LANGUAGES CXX)
11 | 
12 | if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
13 |   message(STATUS "Setting default build type to RelWithDebInfo as none was provided")
14 |   set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "Choose the type of a build" FORCE)
15 | endif()
16 | 
17 | 
18 | find_package(mc_rtc REQUIRED)
19 | find_package(Ceres REQUIRED)
20 | 
21 | add_subdirectory(src)
22 | 
23 | configure_file(etc/ForceSensorCalibration.in.yaml "${CMAKE_CURRENT_BINARY_DIR}/etc/ForceSensorCalibration.yaml")
24 | install(FILES "${CMAKE_CURRENT_BINARY_DIR}/etc/ForceSensorCalibration.yaml" DESTINATION "${MC_CONTROLLER_INSTALL_PREFIX}/etc")
25 | 


--------------------------------------------------------------------------------
/src/states/CalibrationMotionLogging.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <mc_control/fsm/State.h>
 4 | 
 5 | #include <mc_rbdyn/Robot.h>
 6 | 
 7 | #include <RBDyn/Jacobian.h>
 8 | 
 9 | #include <unordered_map>
10 | 
11 | struct CalibrationMotionLogging : mc_control::fsm::State
12 | {
13 | 
14 |   inline void configure(const mc_rtc::Configuration &) override {}
15 | 
16 |   void start(mc_control::fsm::Controller & ctl) override;
17 | 
18 |   bool run(mc_control::fsm::Controller & ctl) override;
19 | 
20 |   void teardown(mc_control::fsm::Controller & ctl) override;
21 | 
22 | private:
23 |   // sensor to calibrate
24 |   std::vector<std::string> sensors_;
25 |   // For each sensor store the jacobian and its decomposition
26 |   struct Jacobian
27 |   {
28 |     Jacobian() {}
29 |     Jacobian(const mc_rbdyn::Robot & robot, const std::string & body)
30 |     : jacobian(robot.mb(), body), svd(Eigen::MatrixXd::Identity(6, jacobian.dof()))
31 |     {
32 |     }
33 |     rbd::Jacobian jacobian;
34 |     Eigen::JacobiSVD<Eigen::MatrixXd> svd;
35 |   };
36 |   std::unordered_map<std::string, Jacobian> jacobians_;
37 |   double singularityThreshold_;
38 |   std::unordered_map<std::string, std::pair<double, double>> measurementsCount_;
39 | };
40 | 


--------------------------------------------------------------------------------
/src/states/ChooseTransition.cpp:
--------------------------------------------------------------------------------
 1 | #include "ChooseTransition.h"
 2 | #include <mc_control/fsm/Controller.h>
 3 | #include <mc_rtc/gui/Button.h>
 4 | 
 5 | void ChooseTransition::configure(const mc_rtc::Configuration & config)
 6 | {
 7 |   config("category", category_);
 8 |   config("actions", actions_);
 9 | }
10 | 
11 | void ChooseTransition::start(mc_control::fsm::Controller & ctl)
12 | {
13 |   using namespace mc_rtc::gui;
14 |   for(const auto & action : actions_)
15 |   {
16 |     ctl.gui()->addElement(category_, Button(action.first, [this, action]() {
17 |                             mc_rtc::log::info("[{}] Action {} chosen, triggering output {}", name(), action.first,
18 |                                               action.second);
19 |                             output(action.second);
20 |                           }));
21 |   }
22 | }
23 | 
24 | bool ChooseTransition::run(mc_control::fsm::Controller &)
25 | {
26 |   if(output().empty())
27 |   {
28 |     return false;
29 |   }
30 |   return true;
31 | }
32 | 
33 | void ChooseTransition::teardown(mc_control::fsm::Controller & ctl)
34 | {
35 |   for(const auto & action : actions_)
36 |   {
37 |     ctl.gui()->removeElement(category_, action.first);
38 |   }
39 | }
40 | 
41 | EXPORT_SINGLE_STATE("ChooseTransition", ChooseTransition)
42 | 


--------------------------------------------------------------------------------
/src/states/InitialPosture.cpp:
--------------------------------------------------------------------------------
 1 | #include "InitialPosture.h"
 2 | #include <mc_rbdyn/rpy_utils.h>
 3 | #include <mc_rtc/io_utils.h>
 4 | #include <boost/filesystem.hpp>
 5 | #include "../ForceSensorCalibration.h"
 6 | 
 7 | void InitialPosture::start(mc_control::fsm::Controller & ctl)
 8 | {
 9 |   const auto & robotConf = ctl.config()("robots")(ctl.robot().name());
10 |   if(!robotConf.has("initial_posture"))
11 |   {
12 |     mc_rtc::log::error_and_throw<std::runtime_error>("[{}] Calibration controller expects an initial_posture entry",
13 |                                                      name());
14 |   }
15 |   const auto & conf(robotConf("initial_posture"));
16 |   auto postureTask = ctl.getPostureTask(ctl.robot().name());
17 |   postureTask->target(conf("target"));
18 |   savedStiffness_ = postureTask->stiffness();
19 |   postureTask->stiffness(conf("stiffness", 10));
20 |   crit_.configure(*postureTask, ctl.solver().dt(), conf("completion", mc_rtc::Configuration{}));
21 | }
22 | 
23 | bool InitialPosture::run(mc_control::fsm::Controller & ctl_)
24 | {
25 |   auto postureTask = ctl_.getPostureTask(ctl_.robot().name());
26 |   if(crit_.completed(*postureTask))
27 |   {
28 |     output("OK");
29 |     return true;
30 |   }
31 |   return false;
32 | }
33 | 
34 | EXPORT_SINGLE_STATE("InitialPosture", InitialPosture)
35 | 


--------------------------------------------------------------------------------
/src/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | set(controller_SRC
 2 |   ForceSensorCalibration.cpp
 3 |   calibrate.cpp
 4 | )
 5 | 
 6 | set(controller_HDR
 7 |   ForceSensorCalibration.h
 8 |   calibrate.h
 9 | )
10 | 
11 | add_library(${PROJECT_NAME} SHARED ${controller_SRC} ${controller_HDR})
12 | set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "-DForceSensorCalibration_EXPORTS")
13 | target_link_libraries(${PROJECT_NAME} PUBLIC mc_rtc::mc_control_fsm)
14 | if(TARGET Ceres::ceres)
15 |   target_link_libraries(${PROJECT_NAME} PUBLIC Ceres::ceres)
16 | else()
17 |   target_link_libraries(${PROJECT_NAME} PUBLIC ${CERES_LIBRARIES})
18 |   target_include_directories(${PROJECT_NAME} PUBLIC ${CERES_INCLUDE_DIRS})
19 | endif()
20 | install(TARGETS ${PROJECT_NAME}
21 |   ARCHIVE DESTINATION "${MC_RTC_LIBDIR}"
22 |   LIBRARY DESTINATION "${MC_RTC_LIBDIR}"
23 |   RUNTIME DESTINATION bin)
24 | 
25 | add_controller(${PROJECT_NAME}_controller lib.cpp "")
26 | set_target_properties(${PROJECT_NAME}_controller PROPERTIES OUTPUT_NAME "${PROJECT_NAME}")
27 | set_target_properties(${PROJECT_NAME}_controller PROPERTIES
28 |   ARCHIVE_OUTPUT_DIRECTORY lib/$<CONFIGURATION>
29 |   LIBRARY_OUTPUT_DIRECTORY lib/$<CONFIGURATION>
30 |   RUNTIME_OUTPUT_DIRECTORY lib/$<CONFIGURATION>)
31 | target_link_libraries(${PROJECT_NAME}_controller PUBLIC ${PROJECT_NAME})
32 | 
33 | add_subdirectory(states)
34 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | BSD 2-Clause License
 2 | 
 3 | Copyright (c) 2012-2019, CNRS-UM LIRMM, CNRS-AIST JRL
 4 | All rights reserved.
 5 | 
 6 | Redistribution and use in source and binary forms, with or without
 7 | modification, are permitted provided that the following conditions are met:
 8 | 
 9 | 1. Redistributions of source code must retain the above copyright notice, this
10 |    list of conditions and the following disclaimer.
11 | 
12 | 2. Redistributions in binary form must reproduce the above copyright notice,
13 |    this list of conditions and the following disclaimer in the documentation
14 |    and/or other materials provided with the distribution.
15 | 
16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 | 


--------------------------------------------------------------------------------
/src/api.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #if defined _WIN32 || defined __CYGWIN__
 4 | #  define ForceSensorCalibration_DLLIMPORT __declspec(dllimport)
 5 | #  define ForceSensorCalibration_DLLEXPORT __declspec(dllexport)
 6 | #  define ForceSensorCalibration_DLLLOCAL
 7 | #else
 8 | // On Linux, for GCC >= 4, tag symbols using GCC extension.
 9 | #  if __GNUC__ >= 4
10 | #    define ForceSensorCalibration_DLLIMPORT __attribute__((visibility("default")))
11 | #    define ForceSensorCalibration_DLLEXPORT __attribute__((visibility("default")))
12 | #    define ForceSensorCalibration_DLLLOCAL __attribute__((visibility("hidden")))
13 | #  else
14 | // Otherwise (GCC < 4 or another compiler is used), export everything.
15 | #    define ForceSensorCalibration_DLLIMPORT
16 | #    define ForceSensorCalibration_DLLEXPORT
17 | #    define ForceSensorCalibration_DLLLOCAL
18 | #  endif // __GNUC__ >= 4
19 | #endif // defined _WIN32 || defined __CYGWIN__
20 | 
21 | #ifdef ForceSensorCalibration_STATIC
22 | // If one is using the library statically, get rid of
23 | // extra information.
24 | #  define ForceSensorCalibration_DLLAPI
25 | #  define ForceSensorCalibration_LOCAL
26 | #else
27 | // Depending on whether one is building or using the
28 | // library define DLLAPI to import or export.
29 | #  ifdef ForceSensorCalibration_EXPORTS
30 | #    define ForceSensorCalibration_DLLAPI ForceSensorCalibration_DLLEXPORT
31 | #  else
32 | #    define ForceSensorCalibration_DLLAPI ForceSensorCalibration_DLLIMPORT
33 | #  endif // ForceSensorCalibration_EXPORTS
34 | #  define ForceSensorCalibration_LOCAL ForceSensorCalibration_DLLLOCAL
35 | #endif // ForceSensorCalibration_STATIC


--------------------------------------------------------------------------------
/src/ForceSensorCalibration.cpp:
--------------------------------------------------------------------------------
 1 | #include "ForceSensorCalibration.h"
 2 | 
 3 | #include <mc_rbdyn/configuration_io.h>
 4 | #include <mc_rtc/io_utils.h>
 5 | 
 6 | ForceSensorCalibration::ForceSensorCalibration(mc_rbdyn::RobotModulePtr rm,
 7 |                                                double dt,
 8 |                                                const mc_rtc::Configuration & config)
 9 | : mc_control::fsm::Controller(rm, dt, config)
10 | {
11 |   auto robotsConfig = config("robots");
12 |   if(!robotsConfig.has(rm->name))
13 |   {
14 |     mc_rtc::log::error_and_throw<std::runtime_error>(
15 |         "No configuration section for {}, add a configuration for this robot before attempting calibration", rm->name);
16 |   }
17 |   auto rConfig = robotsConfig(rm->name);
18 |   mc_rtc::log::info("rConfig:\n{}", rConfig.dump(true, true));
19 |   if(rConfig.has("ObserverPipelines"))
20 |   {
21 |     MCController::createObserverPipelines(rConfig);
22 |   }
23 |   else
24 |   {
25 |     mc_rtc::log::error_and_throw<std::runtime_error>(
26 |         "This controller requires at least an encoder observer for robot {}. In addition, floating base robots also "
27 |         "require estimation of the floating base orientation.",
28 |         robot().name());
29 |   }
30 |   mc_rtc::log::success("ForceSensorCalibration init done");
31 | }
32 | 
33 | bool ForceSensorCalibration::run()
34 | {
35 |   return mc_control::fsm::Controller::run();
36 | }
37 | 
38 | void ForceSensorCalibration::reset(const mc_control::ControllerResetData & reset_data)
39 | {
40 |   mc_control::fsm::Controller::reset(reset_data);
41 |   auto rConfig = config()("robots")(robot().name());
42 |   if(rConfig.has("collisions"))
43 |   {
44 |     addCollisions(robot().name(), "ground", rConfig("collisions"));
45 |   }
46 | }
47 | 


--------------------------------------------------------------------------------
/.github/workflows/build.yml:
--------------------------------------------------------------------------------
 1 | name: CI of mc_force_sensor_calibration_controller
 2 | 
 3 | on:
 4 |   repository_dispatch:
 5 |     types: [build]
 6 |   push:
 7 |     paths-ignore:
 8 |       # Changes to those files don't mandate running CI
 9 |       - ".gitignore"
10 |       - ".github/workflows/package.yml"
11 |       - ".github/workflows/sources/*"
12 |       - "debian/**"
13 |     branches:
14 |       - '**'
15 |   pull_request:
16 |     branches:
17 |       - '**'
18 | 
19 | jobs:
20 |   clang-format:
21 |     runs-on: ubuntu-20.04
22 |     steps:
23 |     - uses: actions/checkout@v2
24 |     - name: Install clang-format-10
25 |       run: |
26 |         sudo apt-get -qq update
27 |         sudo apt-get -qq install clang-format-10
28 |     - name: Run clang-format-check
29 |       run: |
30 |         ./.clang-format-check.sh
31 |   build:
32 |     needs: clang-format
33 |     strategy:
34 |       fail-fast: false
35 |       matrix:
36 |         os: [ubuntu-20.04]
37 |         build-type: [RelWithDebInfo]
38 |         compiler: [gcc, clang]
39 | 
40 |     runs-on: ${{ matrix.os }}
41 |     steps:
42 |     - uses: actions/checkout@v2
43 |       with:
44 |         submodules: recursive
45 |     - name: Install system dependencies
46 |       uses: jrl-umi3218/github-actions/install-dependencies@master
47 |       with:
48 |         compiler: ${{ matrix.compiler }}
49 |         build-type: ${{ matrix.build-type }}
50 |         ubuntu: |
51 |           apt: libeigen3-dev libgflags-dev libgoogle-glog-dev libatlas-base-dev doxygen doxygen-latex libboost-all-dev libmc-rtc-dev
52 |           apt-mirrors:
53 |             mc-rtc:
54 |               cloudsmith: mc-rtc/head
55 |         github: |
56 |           - path: ceres-solver/ceres-solver
57 |     - name: Build and test
58 |       uses: jrl-umi3218/github-actions/build-cmake-project@master
59 |       with:
60 |         compiler: ${{ matrix.compiler }}
61 |         build-type: ${{ matrix.build-type }}
62 | 


--------------------------------------------------------------------------------
/src/calibrate.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | #include <mc_rbdyn/Robot.h>
 3 | #include <ostream>
 4 | 
 5 | #include "Measurement.h"
 6 | 
 7 | struct InitialGuess
 8 | {
 9 |   double mass = 0;
10 |   std::array<double, 3> rpy = {0};
11 |   std::array<double, 3> com = {0};
12 |   std::array<double, 6> offset = {0};
13 | };
14 | 
15 | inline std::ostream & operator<<(std::ostream & os, const InitialGuess & r)
16 | {
17 |   // clang-format off
18 |   return os << fmt::format(
19 | R"(mass        : {}
20 | rpy         : {}, {}, {}
21 | com         : {}, {}, {}
22 | force offset: {}, {}, {}, {}, {}, {})",
23 |       r.mass,
24 |       r.rpy[0], r.rpy[1], r.rpy[2],
25 |       r.com[0], r.com[1], r.com[2],
26 |       r.offset[0], r.offset[1], r.offset[2], r.offset[3], r.offset[4], r.offset[5]);
27 |   // clang-format on
28 | }
29 | 
30 | namespace mc_rtc
31 | {
32 | template<>
33 | struct ConfigurationLoader<InitialGuess>
34 | {
35 |   static InitialGuess load(const mc_rtc::Configuration & config)
36 |   {
37 |     InitialGuess r;
38 |     config("mass", r.mass);
39 |     config("com", r.com);
40 |     config("rpy", r.rpy);
41 |     config("offset", r.offset);
42 |     return r;
43 |   }
44 | 
45 |   static mc_rtc::Configuration save(const InitialGuess & r)
46 |   {
47 |     mc_rtc::Configuration c;
48 |     c.add("mass", r.mass);
49 |     c.add("com", r.com);
50 |     c.add("rpy", r.rpy);
51 |     c.add("offset", r.offset);
52 |     return c;
53 |   }
54 | };
55 | } // namespace mc_rtc
56 | 
57 | struct CalibrationResult
58 | {
59 |   CalibrationResult(const InitialGuess & guess)
60 |   {
61 |     mass = guess.mass;
62 |     rpy = guess.rpy;
63 |     com = guess.com;
64 |     offset = guess.offset;
65 |   }
66 | 
67 |   bool success = false;
68 |   double mass = 0;
69 |   std::array<double, 3> rpy = {0};
70 |   std::array<double, 3> com = {0};
71 |   std::array<double, 6> offset = {0};
72 | };
73 | 
74 | InitialGuess computeInitialGuessFromModel(const mc_rbdyn::Robot & robot,
75 |                                           const std::string & sensor,
76 |                                           bool includeParent = false,
77 |                                           bool verbose = false);
78 | 
79 | CalibrationResult calibrate(const mc_rbdyn::Robot & robot,
80 |                             const std::string & sensor,
81 |                             const Measurements & measurements,
82 |                             const InitialGuess & initialGuess,
83 |                             bool verbose = false);
84 | 


--------------------------------------------------------------------------------
/src/states/PressureCheck.cpp:
--------------------------------------------------------------------------------
 1 | #include "PressureCheck.h"
 2 | #include <mc_rbdyn/rpy_utils.h>
 3 | #include <mc_rtc/io_utils.h>
 4 | #include <boost/filesystem.hpp>
 5 | #include "../ForceSensorCalibration.h"
 6 | 
 7 | #include <mc_rtc/gui.h>
 8 | 
 9 | void PressureCheck::start(mc_control::fsm::Controller & ctl)
10 | {
11 |   const auto & robotConf = ctl.config()("robots")(ctl.robot().name());
12 |   robotConf("maxPressureThreshold", maxPressure_);
13 |   robotConf("forceSensors", forceSensors_);
14 | 
15 |   check(ctl);
16 |   output("OK");
17 | }
18 | 
19 | void PressureCheck::check(mc_control::fsm::Controller & ctl)
20 | {
21 |   success_ = true;
22 |   std::string error = "";
23 |   std::vector<std::string> errorSensors;
24 |   for(const auto & sensor : forceSensors_)
25 |   {
26 |     auto force = ctl.robot().forceSensor(sensor.first).force().norm();
27 |     if(force > maxPressure_)
28 |     {
29 |       error += fmt::format("Excessive force on sensor {} (force (norm) {} > maxForceThreshold {})\n", sensor.first,
30 |                            force, maxPressure_);
31 |       errorSensors.push_back(sensor.first);
32 |       success_ = false;
33 |     }
34 |   }
35 |   if(!success_)
36 |   {
37 |     mc_rtc::log::error("[{}] Excessive force detected on sensors [{}]:\n{}\nPlease make sure that they are not in "
38 |                        "contact and that the calibration motion can be safely executed, then click on \"Continue\".",
39 |                        name(), mc_rtc::io::to_string(errorSensors), error);
40 |     ctl.gui()->removeElement({}, "Error");
41 |     ctl.gui()->removeElement({}, "Continue");
42 |     ctl.gui()->addElement(
43 |         {}, mc_rtc::gui::Label("Error", [errorSensors]() {
44 |           return fmt::format("Excessive force detected on sensors [{}], please make sure that they are not in contact "
45 |                              "and that the calibration motion can be safely executed, then click on \"Continue\".",
46 |                              mc_rtc::io::to_string(errorSensors));
47 |         }));
48 |     ctl.gui()->addElement({}, mc_rtc::gui::Button("Continue", [this, &ctl]() { check(ctl); }));
49 |   }
50 | }
51 | 
52 | bool PressureCheck::run(mc_control::fsm::Controller & ctl)
53 | {
54 |   return success_;
55 | }
56 | 
57 | void PressureCheck::teardown(mc_control::fsm::Controller & ctl)
58 | {
59 |   ctl.gui()->removeElement({}, "Error");
60 |   ctl.gui()->removeElement({}, "Continue");
61 | }
62 | 
63 | EXPORT_SINGLE_STATE("PressureCheck", PressureCheck)
64 | 


--------------------------------------------------------------------------------
/src/states/ShowForces.h:
--------------------------------------------------------------------------------
 1 | #pragma once
 2 | 
 3 | #include <mc_control/fsm/State.h>
 4 | #include <mc_rtc/gui/types.h>
 5 | 
 6 | namespace mc_rbdyn
 7 | {
 8 | struct Robot;
 9 | struct ForceSensor;
10 | } // namespace mc_rbdyn
11 | 
12 | namespace mc_rtc
13 | {
14 | namespace gui
15 | {
16 | struct StateBuilder;
17 | } // namespace gui
18 | } // namespace mc_rtc
19 | 
20 | namespace mc_control
21 | {
22 | namespace fsm
23 | {
24 | struct Controller;
25 | } // namespace fsm
26 | } // namespace mc_control
27 | 
28 | struct ShowForces : mc_control::fsm::State
29 | {
30 | 
31 |   void configure(const mc_rtc::Configuration & config) override;
32 | 
33 |   void start(mc_control::fsm::Controller & ctl) override;
34 | 
35 |   bool run(mc_control::fsm::Controller & ctl) override;
36 | 
37 |   void teardown(mc_control::fsm::Controller & ctl) override;
38 | 
39 |   void saveCalibration(mc_control::fsm::Controller & ctl);
40 | 
41 |   void addWrenchPlot(const std::string & name, mc_rtc::gui::StateBuilder & gui, const mc_rbdyn::ForceSensor & fs);
42 |   void addWrenchWithoutGravityPlot(const std::string & name,
43 |                                    mc_rtc::gui::StateBuilder & gui,
44 |                                    const mc_rbdyn::Robot & robot,
45 |                                    const mc_rbdyn::ForceSensor & fs);
46 |   void addWrenchWithoutGravityPlot(const std::string & name,
47 |                                    const std::string & surface,
48 |                                    mc_rtc::gui::StateBuilder & gui,
49 |                                    const mc_rbdyn::Robot & robot,
50 |                                    const mc_rbdyn::ForceSensor & fs);
51 | 
52 |   void addWrenchVector(const std::string & name,
53 |                        mc_rtc::gui::StateBuilder & gui,
54 |                        const mc_rbdyn::Robot & robot,
55 |                        const mc_rbdyn::ForceSensor & fs);
56 |   void addWrenchWithoutGravityVector(const std::string & name,
57 |                                      mc_rtc::gui::StateBuilder & gui,
58 |                                      const mc_rbdyn::Robot & robot,
59 |                                      const mc_rbdyn::ForceSensor & fs);
60 |   void addWrenchWithoutGravityVector(const std::string & name,
61 |                                      const std::string & surface,
62 |                                      mc_rtc::gui::StateBuilder & gui,
63 |                                      const mc_rbdyn::Robot & robot,
64 |                                      const mc_rbdyn::ForceSensor & fs);
65 | 
66 | private:
67 |   double t_ = 0;
68 |   std::vector<std::pair<std::string, std::string>> sensors_;
69 |   bool stop_ = false;
70 | 
71 |   std::vector<std::string> category_ = {"Calibration", "Show Forces"};
72 |   std::vector<std::string> plots_;
73 |   // Map of force sensor to surface name (chosen in the gui)
74 |   std::map<std::string, std::string> surfaces_;
75 |   double forceScale_ = 1;
76 |   mc_rtc::gui::ForceConfig forceConfig_;
77 | };
78 | 


--------------------------------------------------------------------------------
/src/states/CalibrationMotionLogging.cpp:
--------------------------------------------------------------------------------
 1 | #include "CalibrationMotionLogging.h"
 2 | 
 3 | #include <mc_rbdyn/rpy_utils.h>
 4 | #include <mc_rtc/io_utils.h>
 5 | 
 6 | #include <boost/filesystem.hpp>
 7 | 
 8 | #include "../ForceSensorCalibration.h"
 9 | #include "../Measurement.h"
10 | 
11 | namespace bfs = boost::filesystem;
12 | 
13 | void CalibrationMotionLogging::start(mc_control::fsm::Controller & ctl_)
14 | {
15 |   auto & ctl = static_cast<ForceSensorCalibration &>(ctl_);
16 | 
17 |   auto & robot = ctl.robot();
18 |   auto robotConf = ctl.config()("robots")(robot.name());
19 | 
20 |   singularityThreshold_ = robotConf("SingularityThreshold", ctl.config()("SingularityThreshold"));
21 | 
22 |   if(!robotConf.has("forceSensors"))
23 |   {
24 |     mc_rtc::log::error_and_throw<std::runtime_error>("Calibration controller expects a forceSensors entry");
25 |   }
26 |   sensors_ = robotConf("forceSensors");
27 |   auto outputPath_ = (bfs::temp_directory_path() / fmt::format("calib-force-sensors-data-{}", robot.name())).string();
28 | 
29 |   // Attempt to create the output files
30 |   if(!boost::filesystem::exists(outputPath_))
31 |   {
32 |     if(!boost::filesystem::create_directory(outputPath_))
33 |     {
34 |       mc_rtc::log::error_and_throw<std::runtime_error>("[{}] Could not create output folder {}", name(), outputPath_);
35 |     }
36 |   }
37 | 
38 |   if(!ctl.datastore().has("measurements"))
39 |   {
40 |     ctl.datastore().make<SensorMeasurements>("measurements");
41 |   }
42 |   auto & measurements = ctl.datastore().get<SensorMeasurements>("measurements");
43 |   measurements.clear();
44 |   for(const auto & s : sensors_)
45 |   {
46 |     measurements[s] = {};
47 |     measurementsCount_[s] = {0, 0};
48 |     if(singularityThreshold_ > 0)
49 |     {
50 |       const auto & sensor = ctl_.robot().forceSensor(s);
51 |       jacobians_[s] = {ctl_.robot(), sensor.parentBody()};
52 |     }
53 |   }
54 | }
55 | 
56 | bool CalibrationMotionLogging::run(mc_control::fsm::Controller & ctl_)
57 | {
58 |   auto & robot = ctl_.robot();
59 |   auto & real = ctl_.realRobot();
60 |   auto & measurements = ctl_.datastore().get<SensorMeasurements>("measurements");
61 |   for(const auto & s : sensors_)
62 |   {
63 |     const auto & sensor = robot.forceSensor(s);
64 |     const auto & X_0_p = real.bodyPosW()[real.bodyIndexByName(sensor.parentBody())];
65 |     const auto & measure = sensor.wrench();
66 |     measurementsCount_[s].second++;
67 |     if(singularityThreshold_ > 0)
68 |     {
69 |       auto & j = jacobians_[s];
70 |       const auto & jacMat = j.jacobian.jacobian(robot.mb(), robot.mbc());
71 |       j.svd.compute(jacMat);
72 |       if(j.svd.singularValues().tail(1)(0) > singularityThreshold_)
73 |       {
74 |         measurements[s].push_back({X_0_p, measure});
75 |         measurementsCount_[s].first++;
76 |       }
77 |     }
78 |     else
79 |     {
80 |       measurements[s].push_back({X_0_p, measure});
81 |       measurementsCount_[s].first++;
82 |     }
83 |   }
84 |   output("OK");
85 |   return true;
86 | }
87 | 
88 | void CalibrationMotionLogging::teardown(mc_control::fsm::Controller &)
89 | {
90 |   for(const auto & m : measurementsCount_)
91 |   {
92 |     mc_rtc::log::info("[{}] {} records taken out of {} iterations", m.first, m.second.first, m.second.second);
93 |   }
94 | }
95 | 
96 | EXPORT_SINGLE_STATE("CalibrationMotionLogging", CalibrationMotionLogging)
97 | 


--------------------------------------------------------------------------------
/.clang-format:
--------------------------------------------------------------------------------
  1 | ---
  2 | Language:        Cpp
  3 | AccessModifierOffset: -2
  4 | AlignAfterOpenBracket: Align
  5 | AlignConsecutiveAssignments: false
  6 | AlignConsecutiveDeclarations: false
  7 | AlignEscapedNewlinesLeft: true
  8 | AlignOperands:   true
  9 | AlignTrailingComments: false
 10 | AllowAllParametersOfDeclarationOnNextLine: false
 11 | AllowShortBlocksOnASingleLine: false
 12 | AllowShortCaseLabelsOnASingleLine: false
 13 | AllowShortFunctionsOnASingleLine: Empty
 14 | AllowShortIfStatementsOnASingleLine: true
 15 | AllowShortLoopsOnASingleLine: true
 16 | AlwaysBreakAfterDefinitionReturnType: None
 17 | AlwaysBreakAfterReturnType: None
 18 | AlwaysBreakBeforeMultilineStrings: false
 19 | AlwaysBreakTemplateDeclarations: true
 20 | BinPackArguments: true
 21 | BinPackParameters: false
 22 | BreakBeforeBinaryOperators: NonAssignment
 23 | BreakBeforeBraces: Allman
 24 | BreakBeforeInheritanceComma: false
 25 | BreakBeforeTernaryOperators: true
 26 | BreakConstructorInitializersBeforeComma: false
 27 | BreakConstructorInitializers: BeforeColon
 28 | BreakAfterJavaFieldAnnotations: false
 29 | BreakStringLiterals: true
 30 | ColumnLimit:     120
 31 | CommentPragmas:  '^ IWYU pragma:'
 32 | CompactNamespaces: false
 33 | ConstructorInitializerAllOnOneLineOrOnePerLine: false
 34 | ConstructorInitializerIndentWidth: 0
 35 | ContinuationIndentWidth: 4
 36 | Cpp11BracedListStyle: true
 37 | DerivePointerAlignment: false
 38 | DisableFormat:   false
 39 | ExperimentalAutoDetectBinPacking: false
 40 | FixNamespaceComments: true
 41 | ForEachMacros:
 42 |   - foreach
 43 |   - Q_FOREACH
 44 |   - BOOST_FOREACH
 45 | IncludeBlocks: Preserve
 46 | IncludeCategories:
 47 |   - Regex:           '^(<mc_[a-z]*/)'
 48 |     Priority:        1
 49 |   - Regex:           '^(<Tasks/)'
 50 |     Priority:        2
 51 |   - Regex:           '^(<mc_rbdyn_urdf/)'
 52 |     Priority:        3
 53 |   - Regex:           '^(<RBDyn/)'
 54 |     Priority:        4
 55 |   - Regex:           '^(<SpaceVecAlg/)'
 56 |     Priority:        5
 57 |   - Regex:           '^(<sch-core/)'
 58 |     Priority:        6
 59 |   - Regex:           '^(<boost/)'
 60 |     Priority:        7
 61 |   - Regex:           '^(<(nanomsg|geos|.*_msgs|ros|tf2.*)/)'
 62 |     Priority:        8
 63 |   - Regex:           '.*'
 64 |     Priority:        9
 65 | IncludeIsMainRegex: '(Test)?$'
 66 | IndentCaseLabels: true
 67 | IndentPPDirectives: AfterHash
 68 | IndentWidth:     2
 69 | IndentWrappedFunctionNames: true
 70 | JavaScriptQuotes: Leave
 71 | JavaScriptWrapImports: true
 72 | KeepEmptyLinesAtTheStartOfBlocks: true
 73 | MacroBlockBegin: ''
 74 | MacroBlockEnd:   ''
 75 | MaxEmptyLinesToKeep: 1
 76 | NamespaceIndentation: None
 77 | ObjCBlockIndentWidth: 2
 78 | ObjCSpaceAfterProperty: false
 79 | ObjCSpaceBeforeProtocolList: true
 80 | PenaltyBreakAssignment: 2
 81 | PenaltyBreakBeforeFirstCallParameter: 19
 82 | PenaltyBreakComment: 300
 83 | PenaltyBreakFirstLessLess: 120
 84 | PenaltyBreakString: 1000
 85 | PenaltyExcessCharacter: 150
 86 | PenaltyReturnTypeOnItsOwnLine: 300
 87 | PointerAlignment: Middle
 88 | ReflowComments:  true
 89 | SortIncludes:    true
 90 | SortUsingDeclarations: true
 91 | SpaceAfterCStyleCast: false
 92 | SpaceAfterTemplateKeyword: false
 93 | SpaceBeforeAssignmentOperators: true
 94 | SpaceBeforeParens: Never
 95 | SpaceInEmptyParentheses: false
 96 | SpacesBeforeTrailingComments: 1
 97 | SpacesInAngles:  false
 98 | SpacesInContainerLiterals: true
 99 | SpacesInCStyleCastParentheses: false
100 | SpacesInParentheses: false
101 | SpacesInSquareBrackets: false
102 | Standard:        Cpp11
103 | TabWidth:        2
104 | UseTab:          Never
105 | ...
106 | 
107 | 


--------------------------------------------------------------------------------
/src/states/CalibrationMotion.cpp:
--------------------------------------------------------------------------------
  1 | #include "CalibrationMotion.h"
  2 | #include <mc_filter/utils/clamp.h>
  3 | #include "../ForceSensorCalibration.h"
  4 | 
  5 | void CalibrationMotion::start(mc_control::fsm::Controller & ctl)
  6 | {
  7 |   ctl.datastore().make_call("CalibrationMotion::Stop", [this]() { interrupted_ = true; });
  8 |   auto & robot = ctl.robot();
  9 |   auto robotConf = ctl.config()("robots")(robot.name());
 10 |   if(!robotConf.has("motion"))
 11 |   {
 12 |     mc_rtc::log::error("[{}] Calibration controller expects a joints entry", name());
 13 |     output("FAILURE");
 14 |   }
 15 |   auto conf = robotConf("motion");
 16 |   conf("duration", duration_);
 17 |   conf("percentLimits", percentLimits_);
 18 |   mc_filter::utils::clampInPlace(percentLimits_, 0, 1);
 19 | 
 20 |   auto postureTask = ctl.getPostureTask(robot.name());
 21 |   savedStiffness_ = postureTask->stiffness();
 22 |   postureTask->stiffness(conf("stiffness", 10));
 23 |   constexpr double PI = mc_rtc::constants::PI;
 24 |   for(const auto & jConfig : conf("joints"))
 25 |   {
 26 |     std::string name = jConfig("name");
 27 |     if(!ctl.robot().hasJoint(name))
 28 |     {
 29 |       mc_rtc::log::error("[{}] No joint named \"{}\" in robot \"{}\"", this->name(), name, ctl.robot().name());
 30 |       output("FAILURE");
 31 |     }
 32 |     auto percentLimits = percentLimits_;
 33 |     jConfig("percentLimits", percentLimits);
 34 |     mc_filter::utils::clampInPlace(percentLimits, 0, 1);
 35 |     double period = jConfig("period");
 36 |     auto jidx = robot.jointIndexByName(name);
 37 |     auto start = robot.mbc().q[jidx][0];
 38 |     auto actualLower = robot.ql()[jidx][0];
 39 |     auto actualUpper = robot.qu()[jidx][0];
 40 |     auto actualRange = actualUpper - actualLower;
 41 | 
 42 |     // Reduced range
 43 |     const auto range = percentLimits * actualRange;
 44 |     const auto lower = actualLower + (actualRange - range) / 2;
 45 |     const auto upper = actualUpper - (actualRange - range) / 2;
 46 | 
 47 |     if(start < lower || start > upper)
 48 |     {
 49 |       mc_rtc::log::error("[{}] Starting joint configuration of joint {} [{}] is outside of the reduced limit range "
 50 |                          "[{}, {}] (percentLimits: {}, actual joint limits: [{}, {}]",
 51 |                          this->name(), name, start, lower, upper, percentLimits, actualLower, actualUpper);
 52 |       output("FAILURE");
 53 |     }
 54 | 
 55 |     // compute the starting time such that the joint does not move initially
 56 |     // that is such that f(start_dt) = start
 57 |     // i.e start_dt = f^(-1)(start)
 58 |     double start_dt = period * (acos(sqrt(start - lower) / sqrt(upper - lower))) / PI;
 59 |     jointUpdates_.emplace_back(
 60 |         /* f(t): periodic function that moves the joint between its limits */
 61 |         [this, postureTask, lower, upper, start_dt, period, name]() {
 62 |           auto t = start_dt + dt_;
 63 |           auto q = lower + (upper - lower) * (1 + cos((2 * PI * t) / period)) / 2;
 64 |           postureTask->target({{name, {q}}});
 65 |         });
 66 |   }
 67 | 
 68 |   ctl.gui()->addElement({},
 69 |                         mc_rtc::gui::NumberSlider(
 70 |                             "Progress", [this]() { return dt_; }, [](double) {}, 0, duration_),
 71 |                         mc_rtc::gui::Button("Stop Motion", [this]() {
 72 |                           mc_rtc::log::warning(
 73 |                               "[{}] Motion was interrupted before it's planned duration ({:.2f}/{:.2f}s)", name(), dt_,
 74 |                               duration_);
 75 |                           interrupted_ = true;
 76 |                         }));
 77 | }
 78 | 
 79 | bool CalibrationMotion::run(mc_control::fsm::Controller & ctl_)
 80 | {
 81 |   if(output() == "FAILURE")
 82 |   {
 83 |     return true;
 84 |   }
 85 | 
 86 |   // Update all joint positions
 87 |   for(auto & updateJoint : jointUpdates_)
 88 |   {
 89 |     updateJoint();
 90 |   }
 91 | 
 92 |   if(dt_ > duration_)
 93 |   {
 94 |     dt_ = duration_;
 95 |     auto postureTask = ctl_.getPostureTask(ctl_.robot().name());
 96 |     postureTask->refVel(Eigen::VectorXd{ctl_.robot().mb().nrDof()}.setZero());
 97 |     if(postureTask->speed().norm() < 1e-5)
 98 |     {
 99 |       output("OK");
100 |       return true;
101 |     }
102 |   }
103 |   else if(interrupted_)
104 |   {
105 |     output("INTERRUPTED");
106 |     return true;
107 |   }
108 | 
109 |   dt_ += ctl_.timeStep;
110 |   return false;
111 | }
112 | 
113 | void CalibrationMotion::teardown(mc_control::fsm::Controller & ctl_)
114 | {
115 |   auto postureTask = ctl_.getPostureTask(ctl_.robot().name());
116 |   postureTask->stiffness(savedStiffness_);
117 |   ctl_.gui()->removeElement({}, "Progress");
118 |   ctl_.gui()->removeElement({}, "Stop Motion");
119 |   ctl_.datastore().remove("CalibrationMotion::Stop");
120 | }
121 | 
122 | EXPORT_SINGLE_STATE("CalibrationMotion", CalibrationMotion)
123 | 


--------------------------------------------------------------------------------
/etc/ForceSensorCalibration.in.yaml:
--------------------------------------------------------------------------------
  1 | ---
  2 | # If true, the FSM transitions are managed by an external tool
  3 | Managed: false
  4 | # If true and the FSM is self-managed, transitions should be triggered
  5 | StepByStep: true
  6 | # Change idle behaviour, if true the state is kept until transition,
  7 | # otherwise the FSM holds the last state until transition
  8 | IdleKeepState: true
  9 | # Where to look for state libraries
 10 | StatesLibraries:
 11 | - "@MC_RTC_LIBDIR@/mc_controller/ForceSensorCalibration/states"
 12 | - "@MC_STATES_DEFAULT_INSTALL_PREFIX@"
 13 | - "@MC_STATES_INSTALL_PREFIX@"
 14 | # Where to look for state files
 15 | StatesFiles:
 16 |   # - "@MC_RTC_LIBDIR@/mc_controller/ForceSensorCalibration/states/data"
 17 | - "@MC_STATES_DEFAULT_INSTALL_PREFIX@/data"
 18 | - "@MC_STATES_INSTALL_PREFIX@/data"
 19 | # If true, state factory will be more verbose
 20 | VerboseStateFactory: false
 21 | 
 22 | # Additional robots to load and configuration for each robot
 23 | robots:
 24 |   ground:
 25 |     module: env/ground
 26 | 
 27 |   jvrc1:
 28 |     ObserverPipelines:
 29 |       - name: "EncoderAndBodySensor"
 30 |         observers:
 31 |           - type: Encoder
 32 |           - type: BodySensor
 33 |     forceSensors:
 34 |     - LeftHandForceSensor
 35 |     - RightHandForceSensor
 36 |     - LeftFootForceSensor
 37 |     - RightFootForceSensor
 38 |     maxPressureThreshold: 50
 39 |     initial_posture:
 40 |       completion:
 41 |         eval: 0.06
 42 |       target:
 43 |         ## Arms
 44 |         R_SHOULDER_P: [-0.4]
 45 |         L_SHOULDER_P: [-0.4]
 46 |         R_SHOULDER_R: [-0.5]
 47 |         L_SHOULDER_R: [0.5]
 48 |         R_ELBOW_P: [-1.5]
 49 |         L_ELBOW_P: [-1.5]
 50 |         ## Legs
 51 |         L_HIP_R: [0.25]
 52 |         R_HIP_R: [-0.25]
 53 |     motion:
 54 |       duration: 30
 55 |       stiffness: 10
 56 |       percentLimits: 0.8
 57 |       joints:
 58 |         # Left wrist
 59 |       - name: L_WRIST_R
 60 |         period: 16
 61 |       - name: L_WRIST_Y
 62 |         period: 20
 63 |         # right wrist
 64 |       - name: R_WRIST_R
 65 |         period: 16
 66 |       - name: R_WRIST_Y
 67 |         period: 20
 68 |         # Legs
 69 |       - name: R_ANKLE_P
 70 |         period: 10
 71 |       - name: L_ANKLE_P
 72 |         period: 15
 73 |       - name: R_ANKLE_R
 74 |         period: 10
 75 |       - name: L_ANKLE_R
 76 |         period: 15
 77 | 
 78 | 
 79 | # General constraints, always on
 80 | constraints:
 81 | - type: contact
 82 | - type: kinematics
 83 |   damper: [0.1, 0.01, 0.5]
 84 | - type: compoundJoint
 85 | # Collision constraint
 86 | collisions:
 87 | - type: collision
 88 |   useMinimal: true
 89 | # Initial set of contacts
 90 | contacts: []
 91 | 
 92 | # Implement some additional text states
 93 | states:
 94 |   Calibrate:
 95 |     base: Parallel
 96 |     states: [CalibrationMotionLogging, CalibrationMotion]
 97 | 
 98 |   CheckCalibration:
 99 |     base: Parallel
100 |     states: [CheckResults, CalibrationMotion]
101 | 
102 |   GoHalfSitting:
103 |     base: HalfSitting
104 |     eval: 0.1
105 | 
106 |   Calibration::Choice:
107 |     base: ChooseTransition
108 |     category: ["Calibration"]
109 |     actions:
110 |       Start calibration: Calibrate
111 |       Check calibration: Check
112 |       Show forces: ShowForces
113 | 
114 |   Calibration::FSM::Calibrate:
115 |     base: Meta
116 |     transitions:
117 |     - [InitialPosture, OK, Calibrate, Auto]
118 |     - [Calibrate, OK, RunCalibrationScript, Auto]
119 |     - [Calibrate, FAILURE, GoHalfSitting, Auto]
120 |     - [Calibrate, INTERRUPTED, GoHalfSitting, Auto]
121 |     - [RunCalibrationScript, SUCCESS, CheckCalibration, Auto]
122 |     - [RunCalibrationScript, FAILED, GoHalfSitting, Auto]
123 |     - [CheckCalibration, OK, GoHalfSitting, Auto]
124 |     - [CheckCalibration, INTERRUPTED, GoHalfSitting, Auto]
125 |     - [CheckCalibration, FAILURE, GoHalfSitting, Auto]
126 | 
127 |   Calibration::FSM::Check:
128 |     base: Meta
129 |     transitions:
130 |     - [InitialPosture, OK, CheckCalibration, Auto]
131 |     - [CheckCalibration, OK, GoHalfSitting, Auto]
132 |     - [CheckCalibration, INTERRUPTED, GoHalfSitting, Auto]
133 |     configs:
134 |       CheckCalibration:
135 |         configs:
136 |           CheckResults:
137 |             checkDefault: true
138 | 
139 |   Calibration::FSM::ShowForces:
140 |     base: ShowForces
141 |     forceScale: 10
142 | 
143 | # Transitions map
144 | transitions:
145 |   # Choose what to do
146 | - [PressureCheck, OK, Calibration::Choice, Auto]
147 | - [Calibration::Choice, Calibrate, Calibration::FSM::Calibrate, Auto]
148 | - [Calibration::Choice, Check, Calibration::FSM::Check, Auto]
149 | - [Calibration::Choice, ShowForces, Calibration::FSM::ShowForces, Auto]
150 |   # Go back to choice
151 | - [Calibration::FSM::Calibrate, OK, Calibration::Choice, Auto]
152 | - [Calibration::FSM::ShowForces, OK, Calibration::Choice, Auto]
153 | - [Calibration::FSM::Check, OK, Calibration::Choice, Auto]
154 | # Initial state
155 | init: PressureCheck
156 | 
157 | # Drop measurements from calibration if the smallest singular value on the limb jacobian is smaller
158 | SingularityThreshold: 0 # 0 effectively disable this feature
159 | 
160 | 


--------------------------------------------------------------------------------
/src/states/CheckResults.cpp:
--------------------------------------------------------------------------------
  1 | #include "CheckResults.h"
  2 | #include <mc_rtc/gui/plot.h>
  3 | #include <boost/filesystem.hpp>
  4 | #include "../ForceSensorCalibration.h"
  5 | 
  6 | namespace bfs = boost::filesystem;
  7 | 
  8 | void CheckResults::configure(const mc_rtc::Configuration & config)
  9 | {
 10 |   config("checkDefault", checkDefault_);
 11 | }
 12 | 
 13 | void CheckResults::start(mc_control::fsm::Controller & ctl)
 14 | {
 15 |   auto & robot = ctl.robot();
 16 |   using namespace mc_rtc::gui;
 17 |   using Style = mc_rtc::gui::plot::Style;
 18 | 
 19 |   const auto & robotConf = ctl.config()("robots")(robot.name());
 20 |   if(!robotConf.has("forceSensors"))
 21 |   {
 22 |     mc_rtc::log::error_and_throw<std::runtime_error>("[{}] Calibration controller expects a forceSensors entry",
 23 |                                                      name());
 24 |   }
 25 |   sensors_ = robotConf("forceSensors");
 26 |   double duration = robotConf("motion")("duration", 30);
 27 | 
 28 |   std::string calib_path = "";
 29 |   if(checkDefault_)
 30 |   {
 31 |     calib_path = ctl.robot().module().calib_dir;
 32 |   }
 33 |   else
 34 |   {
 35 |     calib_path = "/tmp/calib-force-sensors-result-" + ctl.robot().name();
 36 |   }
 37 | 
 38 |   // Load new calibration parameters
 39 |   for(const auto & sensorN : sensors_)
 40 |   {
 41 |     const auto filename = calib_path + "/calib_data." + sensorN;
 42 |     mc_rtc::log::info("[{}] Loading calibration file {}", name(), filename);
 43 |     auto & sensor = ctl.robot().data()->forceSensors[ctl.robot().data()->forceSensorsIndex.at(sensorN)];
 44 |     sensor.loadCalibrator(filename, ctl.robot().mbc().gravity);
 45 | 
 46 |     ctl.logger().addLogEntry(sensor.name() + "_calibrated",
 47 |                              [&robot, &sensor]() { return sensor.wrenchWithoutGravity(robot); });
 48 | 
 49 |     ctl.gui()->addPlot(
 50 |         sensorN, plot::X({"t", {t_ + 0, t_ + duration}}, [this]() { return t_; }),
 51 |         plot::Y(
 52 |             "Wrenches calibrated (x)", [&robot, &sensor]() { return sensor.wrenchWithoutGravity(robot).force().x(); },
 53 |             Color::Red, Style::Solid),
 54 |         plot::Y(
 55 |             "Wrenches calibrated (y)", [&robot, &sensor]() { return sensor.wrenchWithoutGravity(robot).force().y(); },
 56 |             Color::Green, Style::Solid),
 57 |         plot::Y(
 58 |             "Wrenches calibrated (y)", [&robot, &sensor]() { return sensor.wrenchWithoutGravity(robot).force().z(); },
 59 |             Color::Blue, Style::Solid),
 60 |         plot::Y(
 61 |             "Wrenches raw(x)", [&sensor]() { return sensor.wrench().force().x(); }, Color::Red, Style::Dashed),
 62 |         plot::Y(
 63 |             "Wrenches raw(y)", [&sensor]() { return sensor.wrench().force().y(); }, Color::Green, Style::Dashed),
 64 |         plot::Y(
 65 |             "Wrenches raw(z)", [&sensor]() { return sensor.wrench().force().z(); }, Color::Blue, Style::Dashed));
 66 |   }
 67 | 
 68 |   ctl.gui()->addElement(
 69 |       {}, Label("Status", []() { return "Check the plots to see if the calibrated measurements are close to zero"; }),
 70 |       Button("Save calibration", [this, &ctl]() { saveCalibration(ctl); }),
 71 |       Button("Finish without saving", [this, &ctl]() { running_ = false; }), Button("Save and finish", [this, &ctl]() {
 72 |         saveCalibration(ctl);
 73 |         running_ = false;
 74 |       }));
 75 | }
 76 | 
 77 | bool CheckResults::run(mc_control::fsm::Controller & ctl_)
 78 | {
 79 |   if(running_)
 80 |   {
 81 |     t_ += ctl_.timeStep;
 82 |     return false;
 83 |   }
 84 |   else
 85 |   {
 86 |     if(ctl_.datastore().has("CalibrationMotion::Stop"))
 87 |     {
 88 |       ctl_.datastore().call("CalibrationMotion::Stop");
 89 |     }
 90 |   }
 91 |   output("OK");
 92 |   return true;
 93 | }
 94 | 
 95 | void CheckResults::saveCalibration(mc_control::fsm::Controller & ctl)
 96 | {
 97 |   mc_rtc::log::info("[ForceSensorCalibration] Saving calibration results");
 98 |   const auto & calib_dir = ctl.robot().module().calib_dir;
 99 |   if(!bfs::exists(calib_dir))
100 |   {
101 |     if(bfs::create_directories(calib_dir))
102 |     {
103 |       mc_rtc::log::info("[{}] Created missing calibration directory {}", name(), calib_dir);
104 |     }
105 |     else
106 |     {
107 |       mc_rtc::log::error("[{}] Failed to create calibration directory {}", name(), calib_dir);
108 |       return;
109 |     }
110 |   }
111 | 
112 |   for(const auto & sensor : sensors_)
113 |   {
114 |     const auto source_path =
115 |         "/tmp/calib-force-sensors-result-" + ctl.robot().name() + "/" + std::string("calib_data." + sensor);
116 |     const auto destination_path = calib_dir + "/" + std::string("calib_data." + sensor);
117 |     try
118 |     {
119 |       bfs::copy_file(source_path, destination_path, bfs::copy_option::overwrite_if_exists);
120 |       mc_rtc::log::success("[ForceSensorCalibration] Calibration file copied to {}", destination_path);
121 |     }
122 |     catch(...)
123 |     {
124 |       mc_rtc::log::warning("[ForceSensorCalibration] Failed to save {} calibration file to {}", sensor,
125 |                            destination_path);
126 |     }
127 |   }
128 | }
129 | 
130 | void CheckResults::teardown(mc_control::fsm::Controller & ctl)
131 | {
132 |   for(const auto & sensor : sensors_)
133 |   {
134 |     ctl.gui()->removePlot(sensor);
135 |     ctl.gui()->removeElement({}, "Status");
136 |     ctl.gui()->removeElement({}, "Save calibration");
137 |     ctl.gui()->removeElement({}, "Finish without saving");
138 |     ctl.gui()->removeElement({}, "Save and finish");
139 |     ctl.logger().removeLogEntry(sensor + "_calibrated");
140 |   }
141 | }
142 | 
143 | EXPORT_SINGLE_STATE("CheckResults", CheckResults)
144 | 


--------------------------------------------------------------------------------
/src/states/RunCalibrationScript.cpp:
--------------------------------------------------------------------------------
  1 | #include "RunCalibrationScript.h"
  2 | 
  3 | #include <mc_rbdyn/rpy_utils.h>
  4 | #include <mc_rtc/io_utils.h>
  5 | 
  6 | #include <boost/filesystem.hpp>
  7 | namespace bfs = boost::filesystem;
  8 | 
  9 | #include "../ForceSensorCalibration.h"
 10 | #include "../calibrate.h"
 11 | 
 12 | #ifdef __linux__
 13 | 
 14 | #include <sched.h>
 15 | 
 16 | void reset_affinity()
 17 | {
 18 |   cpu_set_t cpu_set;
 19 |   CPU_ZERO(&cpu_set);
 20 |   for(unsigned int i = 0; i < std::thread::hardware_concurrency(); ++i) { CPU_SET(i, &cpu_set); }
 21 |   int result = sched_setaffinity(0, sizeof(cpu_set_t), &cpu_set);
 22 |   if(result != 0) { perror("sched_setaffinity"); }
 23 | }
 24 | 
 25 | #else
 26 | 
 27 | void reset_affinity() {}
 28 | 
 29 | #endif
 30 | 
 31 | RunCalibrationScript::~RunCalibrationScript()
 32 | {
 33 |   th_.join();
 34 | }
 35 | 
 36 | void RunCalibrationScript::configure(const mc_rtc::Configuration &) {}
 37 | 
 38 | void RunCalibrationScript::start(mc_control::fsm::Controller & ctl_)
 39 | {
 40 |   auto & robot = ctl_.robot();
 41 |   auto robotConf = ctl_.config()("robots")(robot.name());
 42 |   outputPath_ = "/tmp/calib-force-sensors-result-" + ctl_.robot().name();
 43 | 
 44 |   // Attempt to create the output folder
 45 |   if(!boost::filesystem::exists(outputPath_))
 46 |   {
 47 |     if(!boost::filesystem::create_directory(outputPath_))
 48 |     {
 49 |       mc_rtc::log::error_and_throw<std::runtime_error>("[{}] Could not create output folder {}", name(), outputPath_);
 50 |     }
 51 |   }
 52 | 
 53 |   sensors_ = robotConf("forceSensors");
 54 |   bool verbose = robotConf("verboseSolver", false);
 55 |   std::vector<InitialGuess> guess_;
 56 |   guess_.reserve(sensors_.size());
 57 | 
 58 |   for(const auto & s : sensors_)
 59 |   {
 60 |     InitialGuess initialGuess;
 61 |     // If we have an initial guess section provided and it does not have
 62 |     // autocompute: true then we use the provided initial guess
 63 |     if(robotConf.has("initialGuess") && robotConf("initialGuess").has(s)
 64 |        && !robotConf("initialGuess")(s)("autocompute", false))
 65 |     {
 66 |       initialGuess = robotConf("initialGuess")(s);
 67 |       mc_rtc::log::info("Using provided initial guess for \"{}\":\n{}", s, initialGuess);
 68 |     }
 69 |     else
 70 |     { // No initial guess was provided and we need to autocompute it
 71 |       bool verbose = false;
 72 |       bool includeParent = false;
 73 |       if(robotConf.has("initialGuess") && robotConf("initialGuess").has(s))
 74 |       {
 75 |         robotConf("initialGuess")(s)("verbose", verbose);
 76 |         robotConf("initialGuess")(s)("includeParent", includeParent);
 77 |       }
 78 |       initialGuess = computeInitialGuessFromModel(ctl_.robot(), s, includeParent, verbose);
 79 |       mc_rtc::log::info("Computed initial guess for \"{}\" from model (includeParent: {}, verbose: {}):\n{} ", s,
 80 |                         includeParent, verbose, initialGuess);
 81 |     }
 82 |     guess_.push_back(initialGuess);
 83 |   }
 84 | 
 85 |   auto & measurements = ctl_.datastore().get<SensorMeasurements>("measurements");
 86 |   th_ = std::thread([&, verbose, guess_, this]() {
 87 |     reset_affinity();
 88 |     for(size_t i = 0; i < sensors_.size(); ++i)
 89 |     {
 90 |       const auto & s = sensors_[i];
 91 |       const auto & initialGuess = guess_[i];
 92 |       mc_rtc::log::info("Start calibration optimization for {}", s);
 93 |       auto result = calibrate(ctl_.robot(), s, measurements.at(s), initialGuess, verbose);
 94 |       success_ = result.success && success_;
 95 |       if(result.success)
 96 |       {
 97 |         mc_rtc::log::success("Calibration succeeded for {}", s);
 98 |         bfs::path out(outputPath_);
 99 |         out += "/calib_data." + s;
100 |         std::ofstream ofs(out.string());
101 |         if(!ofs.good())
102 |         {
103 |           mc_rtc::log::error("Could not write temporary calibration file to {}", out.string());
104 |           continue;
105 |         }
106 |         ofs << result.mass << "\n";
107 |         ofs << result.rpy[0] << "\n";
108 |         ofs << result.rpy[1] << "\n";
109 |         ofs << result.rpy[2] << "\n";
110 |         ofs << result.com[0] << "\n";
111 |         ofs << result.com[1] << "\n";
112 |         ofs << result.com[2] << "\n";
113 |         ofs << result.offset[0] << "\n";
114 |         ofs << result.offset[1] << "\n";
115 |         ofs << result.offset[2] << "\n";
116 |         ofs << result.offset[3] << "\n";
117 |         ofs << result.offset[4] << "\n";
118 |         ofs << result.offset[5] << "\n";
119 |         mc_rtc::log::info("Wrote temporary calibration file to {}", out.string());
120 |       }
121 |       else
122 |       {
123 |         mc_rtc::log::error("Calibration failed for {}", s);
124 |       }
125 |     }
126 |     completed_ = true;
127 |   });
128 | #ifndef WIN32
129 |   // Lower thread priority so that it has a lesser priority than the real time
130 |   // thread
131 |   auto th_handle = th_.native_handle();
132 |   int policy = 0;
133 |   sched_param param{};
134 |   pthread_getschedparam(th_handle, &policy, &param);
135 |   param.sched_priority = 10;
136 |   if(pthread_setschedparam(th_handle, SCHED_RR, &param) != 0)
137 |   {
138 |     mc_rtc::log::warning("[{}] Failed to lower calibration thread priority. If you are running on a real-time system, "
139 |                          "this might cause latency to the real-time loop.",
140 |                          name());
141 |   }
142 | #endif
143 | }
144 | 
145 | bool RunCalibrationScript::run(mc_control::fsm::Controller &)
146 | {
147 |   if(completed_)
148 |   {
149 |     if(!success_)
150 |     {
151 |       mc_rtc::log::error("[ForceSensor] Calibration script failed");
152 |       output("FAILED");
153 |       return true;
154 |     }
155 |     mc_rtc::log::info("[ForceSensorCalibration] Calibration files written to {}", outputPath_);
156 |     output("SUCCESS");
157 |     return true;
158 |   }
159 |   return false;
160 | }
161 | 
162 | void RunCalibrationScript::teardown(mc_control::fsm::Controller & ctl_)
163 | {
164 |   ctl_.gui()->removeElement({}, "Status");
165 | }
166 | 
167 | EXPORT_SINGLE_STATE("RunCalibrationScript", RunCalibrationScript)
168 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
  1 | Force Sensor Calibration
  2 | ==
  3 | 
  4 | This controller allows to calibrate robot force sensors to allow `mc_rtc` to remove the effect of gravity due to links attached to the force sensors (grippers/feet).
  5 | 
  6 | Robots currently supported:
  7 | - `HRP4LIRMM`
  8 | - `HRP4J`
  9 | - `HRP2DRC`
 10 | - `HRP5P`
 11 | - `JVRC1`
 12 | - `panda_default`
 13 | 
 14 | To add your own robot, see the [Adding your own robot/configuration](#adding-your-own-robot) section below.
 15 | 
 16 | Requirements
 17 | ==
 18 | 
 19 | Running this controller requires:
 20 | - [Ceres library](https://github.com/ceres-solver/ceres-solver)
 21 |   Note: On ubuntu 16.04 you need [ceres 1.14](https://github.com/ceres-solver/ceres-solver/tree/1.14.x)
 22 | - [mc_rtc](https://github.com/jrl-umi3218/mc_rtc)
 23 | - `C++14`
 24 | 
 25 | How to use
 26 | ==
 27 | 
 28 | To calibrate, simply run this controller and follow the instructions in the GUI.
 29 | 
 30 | ```yaml
 31 | MainRobot: JVRC1 # one of the supported robots
 32 | Enabled: ForceSensorCalibration
 33 | ```
 34 | 
 35 | - As a safety, the controller will first check that none of the sensor readings are above a specified threshold (ensures that floating base robots are in the air)
 36 |   - If the test fails, it'll display a message in the GUI. Click on `Continue` once the robot is in the air
 37 | - Then you will be presented with a `Calibration` tab in the GUI:
 38 |   - **Start calibration**: perform the calibration motion (going to an initial posture, and making each sensor move simultaneously. Once the motion is completed, it'll run the calibration optimization, and move to the next `Check calibration` state
 39 |   - **Check calibration**: loads the calibration results, perform the calibration motion again and displays live plots of the results. The calibrated force is expected to be close to `0N`. You can either save the current calibration results if you are satisfied using `Save calibration` or `Save and finish`, or stop without keeping the calibration results using `Finish without saving`. Note that saving might fail if you don't have the writing rights to the calibration directory.
 40 |   - **Show forces**: offers a GUI tab to display forces as arrows/live plots
 41 |   - Once the check calibration state is finished, the robot will go back to halfsitting.
 42 |   - If there is an error, the robot will go back to halfsitting.
 43 | 
 44 | The expected result in the live plots should look similar to (dotted is the raw uncalibrated sensor measurement, solid is the calibrated force with gravity removed which should be close to zero)
 45 | 
 46 | ![Example calibration result for HRP4](doc/hrp4_calibration_example.png)
 47 | 
 48 | 
 49 | Note for simulation
 50 | ==
 51 | 
 52 | For floating base robots, you need to have the robot in the air. In choreonoid, this can be achieved by changing the root joint type to `fixed` in the robot's `wrl` model.
 53 | For convenience, we provide `*_fixed.cnoid` variants of our main robots (`JVRC1`, `HRP4LIRMM`, `HRP4J`, `HRP5P`), use these variants to run the calibration controller in simulation.
 54 | 
 55 | TroubleShooting
 56 | ==
 57 | 
 58 | **HRP4**:
 59 | 
 60 | Currently the real HRP4 has a force sensor with reading flipped along one of its axis. As a result, the choreonoid simulation and reality must be configured differently:
 61 | 
 62 | - Choreonoid:
 63 |   - Use `*_fixed.cnoid`
 64 |   - Don't forget to use the robot module `HRP4ComanoidChoreonoid` to have the correct force sensor frames
 65 | 
 66 | - Real
 67 |   - Use `HRP4Comanoid` robot module
 68 | 
 69 | Adding your own robot
 70 | ==
 71 | 
 72 | To add your own robot, you should add an additional section with the same name as that of your robot in `etc/ForceSensorCalibration.in.yaml` and sumbit a pull request. Here is a brief summary of the yaml configuration parameters.
 73 | 
 74 | - `ObserverPipelines`: a state observation pipeline see [here](https://jrl-umi3218.github.io/mc_rtc/json.html#Observers/ObserverPipelines) for supported options. The calibration method requires the orientation of force sensors w.r.t gravity to be known. For floating base robots this means that you need to know at least the orientation of the floating base w.r.t gravity (roll/pitch), and the joint position in order to compute the sensor frame orientation from kinematics.
 75 | - `forceSensors`: Vector of force sensor names to calibrate
 76 | - `initialGuess`: [optional] Map of force sensor names to initial calibration parameters
 77 |   ```yaml
 78 |   initialGuess:
 79 |     LeftHandForceSensor: # Provide an initial guess manually
 80 |       com: [0, 0, -0.1]
 81 |       mass: 0.3
 82 |       rpy: [0,0,0]
 83 |       offset: [0,0,0,0,0,0]
 84 |     RightHandForceSensor: # Or automatically compute the initial guess from the robot model
 85 |       # When true, automatically estimate the mass/CoM of the links attached to the force sensor.
 86 |       # This assumes that the mass and CoM of each link attached to the sensor is correct in the model.
 87 |       autocompute: true
 88 |       # In some models (HRP-2Kai, HRP-5P, HRP-4CR, etc) the force sensor parent link actually contains both the sensor and the link attached to the sensor
 89 |       # Thus to obtain a reasonable initial guess, we need to count the force sensor's parent link. This is somewhat incorrect as it also includes the mass of the force sensor itself, but that's the best estimate we can get without changing the robot model.
 90 |       includeParent: true
 91 |       ...
 92 |   ```
 93 |   If no initial guess are provided, one will be automatically computed from the robot model. This assumes that the children links of the link to which the force sensor is attached contain the correct mass and inertia.
 94 | - `maxPressureThreshold`: Prevent calibration motion if any of the force sensors measures more than this threhold (norm of force).
 95 | - `initial_posture`: Initial robot posture from which the calibration motion starts.
 96 |   ```yaml
 97 |   initial_posture:
 98 |     completion:
 99 |       eval: 0.06
100 |     target:
101 |       L_HIP_R: [0.25]
102 |       R_HIP_R: [-0.25]
103 |   ```
104 | - `motion`: The calibration motion should move the joints prior to the force sensor and attempt to put the force sensor in as many orientations as possible. This section makes each joint move in a sinusoidal motion within the joint limits (or subrange of).
105 |   ```yaml
106 |   motion:
107 |     duration: 30
108 |     stiffness: 10  # posture task stiffness
109 |     percentLimits: 0.8  # percentage of joint limits for all joints
110 |     joints:
111 |     - name: R_ANKLE_P
112 |       period: 10
113 |     - name: L_ANKLE_P
114 |       period: 15
115 |     - name: R_ANKLE_R
116 |       period: 10
117 |       percentLimits: 0.5 # per-joint limits (optional)
118 |     - name: L_ANKLE_R
119 |       period: 15
120 |   ```
121 | - `verboseSolver` [default=`false`]: When true show per-iteration results of the solver
122 | - `SinglularityThreshold`: Prevents singular configuration (useful for fixed-based robots)
123 |   - `0` disables this feature
124 | 


--------------------------------------------------------------------------------
/src/calibrate.cpp:
--------------------------------------------------------------------------------
  1 | #include "calibrate.h"
  2 | 
  3 | #include <ceres/ceres.h>
  4 | 
  5 | #include <mc_rtc/constants.h>
  6 | #include <mc_rtc/io_utils.h>
  7 | #include <RBDyn/FK.h>
  8 | #include <SpaceVecAlg/SpaceVecAlg>
  9 | 
 10 | /** We redefined sva::Rot. functions to make them work with non-scalar types */
 11 | 
 12 | template<typename T>
 13 | inline Eigen::Matrix3<T> RotX(T theta)
 14 | {
 15 |   T s = sin(theta), c = cos(theta);
 16 |   return (Eigen::Matrix3<T>() << T(1), T(0), T(0), T(0), c, s, T(0), -s, c).finished();
 17 | }
 18 | 
 19 | template<typename T>
 20 | inline Eigen::Matrix3<T> RotY(T theta)
 21 | {
 22 |   T s = sin(theta), c = cos(theta);
 23 |   return (Eigen::Matrix3<T>() << c, T(0), -s, T(0), T(1), T(0), s, T(0), c).finished();
 24 | }
 25 | 
 26 | template<typename T>
 27 | inline Eigen::Matrix3<T> RotZ(T theta)
 28 | {
 29 |   T s = sin(theta), c = cos(theta);
 30 |   return (Eigen::Matrix3<T>() << c, s, T(0), -s, c, T(0), T(0), T(0), T(1)).finished();
 31 | }
 32 | 
 33 | template<typename T>
 34 | Eigen::Matrix3<T> rpyToMat(const T & r, const T & p, const T & y)
 35 | {
 36 |   return RotX<T>(r) * RotY<T>(p) * RotZ<T>(y);
 37 | }
 38 | 
 39 | struct CostFunctor
 40 | {
 41 |   CostFunctor(const sva::PTransformd & pos, const sva::ForceVecd & measure, const sva::PTransformd & X_p_f)
 42 |   : pos_(pos), measure_(measure), X_p_f_(X_p_f)
 43 |   {
 44 |   }
 45 | 
 46 |   template<typename T>
 47 |   bool operator()(const T * const mass, const T * const rpy, const T * const com, const T * const offset, T * residual)
 48 |       const
 49 |   {
 50 |     const T gravity(mc_rtc::constants::GRAVITY);
 51 |     sva::ForceVec<T> vf(Eigen::Vector3<T>::Zero(), Eigen::Vector3<T>(T(0), T(0), -mass[0] * gravity));
 52 |     sva::PTransform<T> X_s_ds = sva::PTransform<T>(rpyToMat(rpy[0], rpy[1], rpy[2]));
 53 |     sva::PTransform<T> X_p_vb = sva::PTransform<T>(Eigen::Vector3<T>(com[0], com[1], com[2]));
 54 |     sva::ForceVec<T> off = sva::ForceVec<T>(Eigen::Vector3<T>(offset[0], offset[1], offset[2]),
 55 |                                             Eigen::Vector3<T>(offset[3], offset[4], offset[5]));
 56 |     sva::PTransform<T> X_0_p = pos_.cast<T>();
 57 |     sva::PTransform<T> X_0_vb = sva::PTransform<T>(Eigen::Matrix3<T>::Identity(), (X_p_vb * X_0_p).translation());
 58 |     sva::PTransform<T> X_p_ds = X_s_ds * X_p_f_.cast<T>();
 59 |     sva::PTransform<T> X_ds_vb = X_0_vb * (X_p_ds * X_0_p).inv();
 60 |     sva::ForceVec<T> vb_f = off + X_ds_vb.transMul(vf);
 61 |     sva::ForceVec<T> diff = measure_.cast<T>() - vb_f;
 62 |     residual[0] = diff.couple().x();
 63 |     residual[1] = diff.couple().y();
 64 |     residual[2] = diff.couple().z();
 65 |     residual[3] = diff.force().x();
 66 |     residual[4] = diff.force().y();
 67 |     residual[5] = diff.force().z();
 68 |     return true;
 69 |   }
 70 | 
 71 | private:
 72 |   const sva::PTransformd pos_;
 73 |   const sva::ForceVecd measure_;
 74 |   const sva::PTransformd X_p_f_;
 75 | };
 76 | 
 77 | struct Minimize
 78 | {
 79 |   template<typename T>
 80 |   bool operator()(const T * const x, T * residual) const
 81 |   {
 82 |     residual[0] = x[0];
 83 |     residual[1] = x[1];
 84 |     residual[2] = x[2];
 85 |     return true;
 86 |   }
 87 | };
 88 | 
 89 | inline std::vector<std::string> getSuccessorBodies(const mc_rbdyn::Robot & robot_,
 90 |                                                    const std::string & rootBody,
 91 |                                                    bool includeRoot = false)
 92 | {
 93 |   auto & robot = const_cast<mc_rbdyn::Robot &>(robot_); // For successorJoints,
 94 |                                                         // should be const
 95 |   auto bIdx = robot.bodyIndexByName(rootBody);
 96 |   // Graph of successor joint built using robot's root as the root of the graph
 97 |   // This returns a map of
 98 |   // - BODY1 -> [SUCCESSOR JOINT1 of BODY1, SUCCESSOR JOINT2 of BODY1.. ]
 99 |   // - BODYN -> [SUCCESSOR JOINT1 of BODYN, SUCCESSOR JOINT2 of BODYN.. ]
100 |   auto successorJointsGraph = robot.mbg().successorJoints(robot.mb().body(0).name());
101 | 
102 |   std::function<std::vector<std::string>(const std::vector<std::string> & succJoints)> computeSuccBodyNames;
103 | 
104 |   std::vector<std::string> successorBodyNames;
105 |   if(includeRoot)
106 |   {
107 |     successorBodyNames.push_back(rootBody);
108 |   }
109 |   computeSuccBodyNames = [&successorBodyNames, &successorJointsGraph, &robot,
110 |                           &computeSuccBodyNames](const std::vector<std::string> & succJoints) {
111 |     for(const auto & joint : succJoints)
112 |     {
113 |       auto successorBodyIdx = robot.mb().successor(robot.mb().jointIndexByName(joint));
114 |       const auto & successorBodyName = robot.mb().body(successorBodyIdx).name();
115 |       const auto & successorJoints = successorJointsGraph.at(successorBodyName);
116 |       // Name of the successor body of this joint + name of all its successors
117 |       successorBodyNames.push_back(successorBodyName);
118 |       computeSuccBodyNames(successorJoints);
119 |     }
120 |     return successorBodyNames;
121 |   };
122 | 
123 |   return computeSuccBodyNames(successorJointsGraph[rootBody]);
124 | }
125 | 
126 | CalibrationResult calibrate(const mc_rbdyn::Robot & robot,
127 |                             const std::string & sensorN,
128 |                             const Measurements & measurements,
129 |                             const InitialGuess & initialGuess,
130 |                             bool verbose)
131 | {
132 |   CalibrationResult result{initialGuess};
133 |   const auto & sensor = robot.forceSensor(sensorN);
134 | 
135 |   // Build the problem.
136 |   ceres::Problem problem;
137 | 
138 |   auto & mass = result.mass;
139 |   auto * com = result.com.data();
140 |   auto * rpy = result.rpy.data();
141 |   auto * offset = result.offset.data();
142 | 
143 |   // For each measurement add a residual block
144 |   for(const auto & measurement : measurements)
145 |   {
146 |     const auto & pos = measurement.X_0_p;
147 |     const auto & f = measurement.measure;
148 |     ceres::CostFunction * cost_function =
149 |         new ceres::AutoDiffCostFunction<CostFunctor, 6, 1, 3, 3, 6>(new CostFunctor(pos, f, sensor.X_p_f()));
150 |     problem.AddResidualBlock(cost_function, new ceres::CauchyLoss(0.5), &mass, rpy, com, offset);
151 |   }
152 |   ceres::CostFunction * min_rpy = new ceres::AutoDiffCostFunction<Minimize, 3, 3>(new Minimize());
153 |   problem.AddResidualBlock(min_rpy, nullptr, rpy);
154 |   problem.SetParameterLowerBound(&mass, 0, 0);
155 |   problem.SetParameterLowerBound(rpy, 0, -2 * M_PI);
156 |   problem.SetParameterLowerBound(rpy, 1, -2 * M_PI);
157 |   problem.SetParameterLowerBound(rpy, 2, -2 * M_PI);
158 |   problem.SetParameterUpperBound(rpy, 0, 2 * M_PI);
159 |   problem.SetParameterUpperBound(rpy, 1, 2 * M_PI);
160 |   problem.SetParameterUpperBound(rpy, 2, 2 * M_PI);
161 | 
162 |   // Run the solver!
163 |   ceres::Solver::Options options;
164 |   options.linear_solver_type = ceres::DENSE_QR;
165 |   options.minimizer_progress_to_stdout = verbose;
166 |   options.max_num_iterations = 1000;
167 |   ceres::Solver::Summary summary;
168 |   Solve(options, &problem, &summary);
169 |   result.success = summary.IsSolutionUsable();
170 | 
171 |   // clang-format off
172 |   mc_rtc::log::info(
173 | R"({},
174 | success     : {}
175 | mass        : {}
176 | rpy         : {}, {}, {}
177 | com         : {}, {}, {}
178 | force offset: {}, {}, {}, {}, {}, {})",
179 |       summary.BriefReport(),
180 |       result.success,
181 |       result.mass,
182 |       result.rpy[0], result.rpy[1], result.rpy[2],
183 |       result.com[0], result.com[1], result.com[2],
184 |       result.offset[0], result.offset[1], result.offset[2], result.offset[3], result.offset[4], result.offset[5]);
185 |   // clang-format on
186 |   return result;
187 | }
188 | 
189 | InitialGuess computeInitialGuessFromModel(const mc_rbdyn::Robot & robot,
190 |                                           const std::string & sensorN,
191 |                                           bool includeParent,
192 |                                           bool verbose)
193 | {
194 |   InitialGuess guess;
195 |   const auto & sensor = robot.forceSensor(sensorN);
196 |   const auto & X_0_parent = robot.bodyPosW(sensor.parentBody());
197 | 
198 |   // Compute initial guess from the robot model:
199 |   // - mass: mass of all links under the force sensor. This assumes:
200 |   //   1/ That the force sensor is attached to its parent link in the model (and
201 |   //   thus that its mass is accounted for in the parent link)
202 |   //   2/ That the mass of all child links under the force sensor is correct
203 |   // FIXME for now include the parent body in the computation as HRP2 model is
204 |   // buggy. Remove "true" argument once fixed
205 |   auto successorBodies = getSuccessorBodies(robot, sensor.parentBody(), includeParent);
206 |   double totalMass = 0;
207 |   Eigen::Vector3d com = Eigen::Vector3d::Zero();
208 |   if(successorBodies.size())
209 |   {
210 |     auto & mb = robot.mb();
211 |     auto & mbc = robot.mbc();
212 |     for(const auto & bodyName : successorBodies)
213 |     {
214 |       auto bodyIndex = robot.mb().bodyIndexByName(bodyName);
215 |       const auto & body = robot.mb().body(bodyIndex);
216 |       double mass = body.inertia().mass();
217 |       totalMass += mass;
218 |       auto X_parent_body = mbc.bodyPosW[bodyIndex] * X_0_parent.inv();
219 |       sva::PTransformd scaledBobyPosW(X_parent_body.rotation(), mass * X_parent_body.translation());
220 |       com += (sva::PTransformd(body.inertia().momentum()) * scaledBobyPosW).translation();
221 |       if(verbose)
222 |       {
223 |         mc_rtc::log::info("[Initial Guess] Body: {}, Mass: {}", body.name(), mass);
224 |       }
225 |     }
226 |     if(totalMass > 0)
227 |     {
228 |       com /= totalMass;
229 |     }
230 |     else
231 |     {
232 |       mc_rtc::log::warning(
233 |           "Warning: no mass provided for the bodies attached to force sensor \"{}\", assuming mass = 0 and CoM=[0,0,0]",
234 |           sensor.name());
235 |     }
236 |   }
237 | 
238 |   guess.mass = totalMass;
239 |   guess.com[0] = com[0];
240 |   guess.com[1] = com[1];
241 |   guess.com[2] = com[2];
242 | 
243 |   return guess;
244 | }
245 | 


--------------------------------------------------------------------------------
/src/states/ShowForces.cpp:
--------------------------------------------------------------------------------
  1 | #include "ShowForces.h"
  2 | #include <mc_control/fsm/Controller.h>
  3 | #include <mc_rbdyn/Robot.h>
  4 | #include <mc_rtc/gui.h>
  5 | 
  6 | using namespace mc_rtc::gui;
  7 | using Style = mc_rtc::gui::plot::Style;
  8 | 
  9 | void ShowForces::configure(const mc_rtc::Configuration & config)
 10 | {
 11 |   config("category", category_);
 12 |   config("forceScale", forceScale_);
 13 | }
 14 | 
 15 | void ShowForces::addWrenchPlot(const std::string & name,
 16 |                                mc_rtc::gui::StateBuilder & gui,
 17 |                                const mc_rbdyn::ForceSensor & fs)
 18 | {
 19 |   gui.addPlot(name, plot::X("t", [this]() { return t_; }),
 20 |               plot::Y(
 21 |                   name + " (x)", [&fs]() { return fs.wrench().force().x(); }, Color::Red, Style::Dashed),
 22 |               plot::Y(
 23 |                   name + " (y)", [&fs]() { return fs.wrench().force().y(); }, Color::Green, Style::Dashed),
 24 |               plot::Y(
 25 |                   name + " (z)", [&fs]() { return fs.wrench().force().z(); }, Color::Blue, Style::Dashed));
 26 |   plots_.push_back(name);
 27 | }
 28 | 
 29 | void ShowForces::addWrenchWithoutGravityPlot(const std::string & name,
 30 |                                              mc_rtc::gui::StateBuilder & gui,
 31 |                                              const mc_rbdyn::Robot & robot,
 32 |                                              const mc_rbdyn::ForceSensor & fs)
 33 | {
 34 |   gui.addPlot(name, plot::X("t", [this]() { return t_; }),
 35 |               plot::Y(
 36 |                   name + " (x)", [&robot, &fs]() { return fs.wrenchWithoutGravity(robot).force().x(); }, Color::Red,
 37 |                   Style::Dashed),
 38 |               plot::Y(
 39 |                   name + " (y)", [&robot, &fs]() { return fs.wrenchWithoutGravity(robot).force().y(); }, Color::Green,
 40 |                   Style::Dashed),
 41 |               plot::Y(
 42 |                   name + " (z)", [&robot, &fs]() { return fs.wrenchWithoutGravity(robot).force().z(); }, Color::Blue,
 43 |                   Style::Dashed));
 44 |   plots_.push_back(name);
 45 | }
 46 | 
 47 | void ShowForces::addWrenchWithoutGravityPlot(const std::string & name,
 48 |                                              const std::string & surface,
 49 |                                              mc_rtc::gui::StateBuilder & gui,
 50 |                                              const mc_rbdyn::Robot & robot,
 51 |                                              const mc_rbdyn::ForceSensor & fs)
 52 | {
 53 |   gui.addPlot(name, plot::X("t", [this]() { return t_; }),
 54 |               plot::Y(
 55 |                   name + " (x)", [&robot, &fs, surface]() { return robot.surfaceWrench(surface).force().x(); },
 56 |                   Color::Red, Style::Dashed),
 57 |               plot::Y(
 58 |                   name + " (y)", [&robot, &fs, surface]() { return robot.surfaceWrench(surface).force().y(); },
 59 |                   Color::Green, Style::Dashed),
 60 |               plot::Y(
 61 |                   name + " (z)", [&robot, &fs, surface]() { return robot.surfaceWrench(surface).force().z(); },
 62 |                   Color::Blue, Style::Dashed));
 63 |   plots_.push_back(name);
 64 | }
 65 | 
 66 | void ShowForces::addWrenchVector(const std::string & name,
 67 |                                  mc_rtc::gui::StateBuilder & gui,
 68 |                                  const mc_rbdyn::Robot & robot,
 69 |                                  const mc_rbdyn::ForceSensor & fs)
 70 | {
 71 |   gui.addElement(category_,
 72 |                  Force(
 73 |                      name, forceConfig_, [&fs]() { return fs.wrench(); }, [&fs, &robot]() { return fs.X_0_f(robot); }));
 74 | }
 75 | 
 76 | void ShowForces::addWrenchWithoutGravityVector(const std::string & name,
 77 |                                                mc_rtc::gui::StateBuilder & gui,
 78 |                                                const mc_rbdyn::Robot & robot,
 79 |                                                const mc_rbdyn::ForceSensor & fs)
 80 | {
 81 |   gui.addElement(category_, Force(
 82 |                                 name, forceConfig_, [&fs, &robot]() { return fs.wrenchWithoutGravity(robot); },
 83 |                                 [&fs, &robot]() { return fs.X_0_f(robot); }));
 84 | }
 85 | 
 86 | void ShowForces::addWrenchWithoutGravityVector(const std::string & name,
 87 |                                                const std::string & surface,
 88 |                                                mc_rtc::gui::StateBuilder & gui,
 89 |                                                const mc_rbdyn::Robot & robot,
 90 |                                                const mc_rbdyn::ForceSensor & fs)
 91 | {
 92 |   gui.addElement(category_, Force(
 93 |                                 name, forceConfig_, [&fs, &robot, surface]() { return robot.surfaceWrench(surface); },
 94 |                                 [&fs, &robot, surface]() { return robot.surfacePose(surface); }));
 95 | }
 96 | 
 97 | void ShowForces::start(mc_control::fsm::Controller & ctl)
 98 | {
 99 |   auto & robot = ctl.robot();
100 | 
101 |   ctl.gui()->addElement(category_, Button("Stop", [this]() { stop_ = true; }));
102 | 
103 |   forceConfig_.force_scale *= forceScale_;
104 |   for(const auto & fs : robot.forceSensors())
105 |   {
106 |     const auto & name = fs.name();
107 |     auto fsCategory = category_;
108 |     fsCategory.push_back(fs.name());
109 |     ctl.gui()->addElement(
110 |         fsCategory, ElementsStacking::Horizontal,
111 |         Button("Plot wrench (raw)", [this, &ctl, &fs]() { addWrenchPlot("Wrench " + fs.name(), *ctl.gui(), fs); }),
112 |         Button("Stop wrench (raw)", [this, &ctl, &fs]() { ctl.gui()->removePlot("Wrench " + fs.name()); }));
113 | 
114 |     ctl.gui()->addElement(
115 |         fsCategory, ElementsStacking::Horizontal,
116 |         Button("Plot wrench (without gravity)",
117 |                [this, &ctl, &fs]() {
118 |                  addWrenchWithoutGravityPlot("Wrench without gravity " + fs.name(), *ctl.gui(), ctl.robot(), fs);
119 |                }),
120 |         Button("Stop wrench (without gravity)", [this, &ctl, &fs]() { ctl.gui()->removePlot("Wrench " + fs.name()); }));
121 | 
122 |     ctl.gui()->addElement(
123 |         fsCategory, ElementsStacking::Horizontal,
124 |         Button("Force (raw)",
125 |                [this, &ctl, &robot, &fs]() { addWrenchVector("Force " + fs.name(), *ctl.gui(), robot, fs); }),
126 |         Button("Remove (raw)", [this, &ctl, &fs]() { ctl.gui()->removeElement(category_, "Force " + fs.name()); }));
127 | 
128 |     ctl.gui()->addElement(fsCategory, ElementsStacking::Horizontal,
129 |                           Button("Force (without gravity)",
130 |                                  [this, &ctl, &robot, &fs]() {
131 |                                    addWrenchWithoutGravityVector("Force " + fs.name() + " (without gravity)",
132 |                                                                  *ctl.gui(), robot, fs);
133 |                                  }),
134 |                           Button("Remove (without gravity)", [this, &ctl, &fs]() {
135 |                             ctl.gui()->removeElement(category_, "Force " + fs.name() + " (without gravity)");
136 |                           }));
137 | 
138 |     std::vector<std::string> surfaces;
139 |     for(const auto & surface : robot.surfaces())
140 |     {
141 |       try
142 |       {
143 |         const auto & body = surface.second->bodyName();
144 |         const auto & fs =
145 |             robot.bodyHasForceSensor(body) ? robot.bodyForceSensor(body) : robot.indirectBodyForceSensor(body);
146 |         if(fs.name() == name)
147 |         {
148 |           surfaces.push_back(surface.first);
149 |         }
150 |       }
151 |       catch(...)
152 |       {
153 |       }
154 |     }
155 |     if(surfaces.size())
156 |     {
157 |       surfaces_[name] = surfaces.front();
158 |       ctl.gui()->addElement(fsCategory, mc_rtc::gui::ComboInput(
159 |                                             "Surface", surfaces, [this, name]() { return surfaces_[name]; },
160 |                                             [this, name](const std::string & surface) {
161 |                                               mc_rtc::log::info("[ShowForces] Surface {} selected", surface);
162 |                                               surfaces_[name] = surface;
163 |                                             }));
164 |       // mc  _rtc::gui::FormDataComboInput{"R0 surface", false, {"surfaces", "$R0"}},
165 | 
166 |       ctl.gui()->addElement(fsCategory, ElementsStacking::Horizontal,
167 |                             Button("Plot surface wrench (without gravity)",
168 |                                    [this, &ctl, &fs]() {
169 |                                      addWrenchWithoutGravityPlot("Wrench without gravity " + fs.name(),
170 |                                                                  surfaces_[fs.name()], *ctl.gui(), ctl.robot(), fs);
171 |                                    }),
172 |                             Button("Stop surface wrench (without gravity)",
173 |                                    [this, &ctl, &fs]() { ctl.gui()->removePlot("Wrench " + fs.name()); }));
174 |       ctl.gui()->addElement(fsCategory, ElementsStacking::Horizontal,
175 |                             Button("Surface Force (without gravity)",
176 |                                    [this, &ctl, &robot, &fs]() {
177 |                                      addWrenchWithoutGravityVector("Surface Force " + fs.name() + " (without gravity)",
178 |                                                                    surfaces_[fs.name()], *ctl.gui(), robot, fs);
179 |                                    }),
180 |                             Button("Remove Surface Force (without gravity)", [this, &ctl, &fs]() {
181 |                               ctl.gui()->removeElement(category_, "Surface Force " + fs.name() + " (without gravity)");
182 |                             }));
183 |     }
184 |   }
185 |   output("OK");
186 | }
187 | 
188 | bool ShowForces::run(mc_control::fsm::Controller & ctl_)
189 | {
190 |   t_ += ctl_.timeStep;
191 |   return stop_;
192 | }
193 | 
194 | void ShowForces::teardown(mc_control::fsm::Controller & ctl)
195 | {
196 |   ctl.gui()->removeElement(category_, "Stop");
197 |   for(const auto & plot : plots_)
198 |   {
199 |     ctl.gui()->removePlot(plot);
200 |   }
201 |   for(const auto & fs : ctl.robot().forceSensors())
202 |   {
203 |     const auto & name = fs.name();
204 |     auto fsCategory = category_;
205 |     fsCategory.push_back(name);
206 |     ctl.gui()->removeCategory(fsCategory);
207 |   }
208 | }
209 | 
210 | EXPORT_SINGLE_STATE("ShowForces", ShowForces)
211 | 


--------------------------------------------------------------------------------