diff --git a/CMakeLists.txt b/CMakeLists.txt index e5acda09..e3c13a96 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -63,6 +63,7 @@ add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_BIND USE_LINK_PATH) include(AddWalkingControllersLibrary) +include(AddWalkingControllersYARPThrift) include(AddWalkingControllersApplication) add_subdirectory(src) diff --git a/cmake/AddWalkingControllersYARPThrift.cmake b/cmake/AddWalkingControllersYARPThrift.cmake new file mode 100644 index 00000000..2ed86cf9 --- /dev/null +++ b/cmake/AddWalkingControllersYARPThrift.cmake @@ -0,0 +1,64 @@ +# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# BSD-3-Clause license. + +function(add_walking_controllers_yarp_thrift) + + set(options ) + set(oneValueArgs NAME INSTALLATION_FOLDER THRIFT) + set(multiValueArgs ) + + set(prefix "walking_component") + + cmake_parse_arguments(${prefix} + "${options}" + "${oneValueArgs}" + "${multiValueArgs}" + ${ARGN}) + + set(name ${${prefix}_NAME}) + set(installation_folder ${${prefix}_INSTALLATION_FOLDER}) + set(thrift ${${prefix}_THRIFT}) + + if(NOT installation_folder) + set(installation_folder ${name}) + endif() + + yarp_idl_to_dir(INPUT_FILES ${thrift} + OUTPUT_DIR ${CMAKE_CURRENT_BINARY_DIR}/autogenerated + SOURCES_VAR AUTOGENERATE_SRC + HEADERS_VAR AUTOGENERATE_HDR + INCLUDE_DIRS_VAR AUTOGENERATE_INCLUDE_DIRS + VERBOSE) + + add_library(${name} ${AUTOGENERATE_SRC} ${AUTOGENERATE_HDR}) + + target_link_libraries(${name} PUBLIC YARP::YARP_OS) + + add_library(WalkingControllers::${name} ALIAS ${name}) + + set_target_properties(${name} PROPERTIES + OUTPUT_NAME "${PROJECT_NAME}${name}" + VERSION ${WalkingControllers_VERSION} + PUBLIC_HEADER "${AUTOGENERATE_HDR}" + ) + + target_include_directories(${name} PUBLIC + "$" + "$/${CMAKE_INSTALL_INCLUDEDIR}>") + + # Specify installation targets, typology and destination folders. + install(TARGETS ${name} + EXPORT ${PROJECT_NAME} + COMPONENT runtime + LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib + ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT lib + RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" COMPONENT bin + PUBLIC_HEADER DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/WalkingControllers/${installation_folder}" COMPONENT dev + PRIVATE_HEADER DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/WalkingControllers/${installation_folder}/impl" COMPONENT dev) + + set_property(GLOBAL APPEND PROPERTY WalkingControllers_TARGETS ${name}) + + message(STATUS "Created target ${name} for export ${PROJECT_NAME}.") + +endfunction() diff --git a/cmake/WalkingControllersDependencies.cmake b/cmake/WalkingControllersDependencies.cmake index 07f92e55..893e7a91 100644 --- a/cmake/WalkingControllersDependencies.cmake +++ b/cmake/WalkingControllersDependencies.cmake @@ -10,4 +10,7 @@ find_package(Eigen3 3.2.92 REQUIRED) find_package(UnicyclePlanner 0.5.0 REQUIRED) find_package(OsqpEigen 0.4.0 REQUIRED) find_package(qpOASES REQUIRED) -find_package(BipedalLocomotionFramework 0.6.0 COMPONENTS VectorsCollection REQUIRED) +find_package(BipedalLocomotionFramework 0.9.0 + COMPONENTS VectorsCollection IK ParametersHandlerYarpImplementation + ContinuousDynamicalSystem ManifConversions + ParametersHandlerYarpImplementation REQUIRED) diff --git a/src/KinDynWrapper/include/WalkingControllers/KinDynWrapper/Wrapper.h b/src/KinDynWrapper/include/WalkingControllers/KinDynWrapper/Wrapper.h index c9453188..f84c1718 100644 --- a/src/KinDynWrapper/include/WalkingControllers/KinDynWrapper/Wrapper.h +++ b/src/KinDynWrapper/include/WalkingControllers/KinDynWrapper/Wrapper.h @@ -29,7 +29,7 @@ namespace WalkingControllers class WalkingFK { - iDynTree::KinDynComputations m_kinDyn; /**< KinDynComputations solver. */ + std::shared_ptr m_kinDyn; /**< KinDynComputations solver. */ bool m_useExternalRobotBase; /**< is external estimator for the base of robot used? */ iDynTree::FreeFloatingGeneralizedTorques m_generalizedBiasForces; @@ -267,6 +267,8 @@ namespace WalkingControllers * @return the joint position expressed in radians */ const iDynTree::VectorDynSize& getJointPos(); + + std::shared_ptr getKinDyn(); }; }; #endif diff --git a/src/KinDynWrapper/src/Wrapper.cpp b/src/KinDynWrapper/src/Wrapper.cpp index 1969e709..2ef363a9 100644 --- a/src/KinDynWrapper/src/Wrapper.cpp +++ b/src/KinDynWrapper/src/Wrapper.cpp @@ -10,6 +10,8 @@ #include // YARP +#include +#include #include #include @@ -26,13 +28,13 @@ using namespace WalkingControllers; bool WalkingFK::setRobotModel(const iDynTree::Model& model) { - if(!m_kinDyn.loadRobotModel(model)) + if(!m_kinDyn->loadRobotModel(model)) { yError() << "[WalkingFK::setRobotModel] Error while loading into KinDynComputations object."; return false; } - m_kinDyn.setFrameVelocityRepresentation(iDynTree::MIXED_REPRESENTATION); + m_kinDyn->setFrameVelocityRepresentation(iDynTree::MIXED_REPRESENTATION); // initialize some quantities needed for the first step m_prevContactLeft = false; @@ -42,7 +44,7 @@ bool WalkingFK::setRobotModel(const iDynTree::Model& model) bool WalkingFK::setBaseFrame(const std::string& baseFrame, const std::string& name) { - if(!m_kinDyn.isValid()) + if(!m_kinDyn->isValid()) { yError() << "[WalkingFK::setBaseFrames] Please set the Robot model before calling this method."; return false; @@ -53,17 +55,17 @@ bool WalkingFK::setBaseFrame(const std::string& baseFrame, const std::string& na // - left_foot when the left foot is the stance foot; // - right_foot when the right foot is the stance foot. //.-.root when the external base supposed to be used - m_frameBaseIndex = m_kinDyn.model().getFrameIndex(baseFrame); + m_frameBaseIndex = m_kinDyn->model().getFrameIndex(baseFrame); if(m_frameBaseIndex == iDynTree::FRAME_INVALID_INDEX) { yError() << "[setBaseFrames] Unable to find the frame named: " << baseFrame; return false; } - iDynTree::LinkIndex linkBaseIndex = m_kinDyn.model().getFrameLink(m_frameBaseIndex); + iDynTree::LinkIndex linkBaseIndex = m_kinDyn->model().getFrameLink(m_frameBaseIndex); - std::string baseFrameName = m_kinDyn.model().getLinkName(linkBaseIndex); + std::string baseFrameName = m_kinDyn->model().getLinkName(linkBaseIndex); m_baseFrames.insert({name, std::make_pair(baseFrameName, - m_kinDyn.getRelativeTransform(m_frameBaseIndex, + m_kinDyn->getRelativeTransform(m_frameBaseIndex, linkBaseIndex))}); return true; @@ -72,6 +74,8 @@ bool WalkingFK::setBaseFrame(const std::string& baseFrame, const std::string& na bool WalkingFK::initialize(const yarp::os::Searchable& config, const iDynTree::Model& model) { + m_kinDyn = std::make_shared(); + // check if the config is empty if(!setRobotModel(model)) { @@ -101,7 +105,7 @@ bool WalkingFK::initialize(const yarp::os::Searchable& config, return false; } - m_frameLeftIndex = m_kinDyn.model().getFrameIndex(lFootFrame); + m_frameLeftIndex = m_kinDyn->model().getFrameIndex(lFootFrame); if(m_frameLeftIndex == iDynTree::FRAME_INVALID_INDEX) { yError() << "[WalkingFK::initialize] Unable to find the frame named: " << lFootFrame; @@ -110,7 +114,7 @@ bool WalkingFK::initialize(const yarp::os::Searchable& config, // set base frames - m_frameRightIndex = m_kinDyn.model().getFrameIndex(rFootFrame); + m_frameRightIndex = m_kinDyn->model().getFrameIndex(rFootFrame); if(m_frameRightIndex == iDynTree::FRAME_INVALID_INDEX) { yError() << "[WalkingFK::initialize] Unable to find the frame named: " << rFootFrame; @@ -124,7 +128,7 @@ bool WalkingFK::initialize(const yarp::os::Searchable& config, yError() << "[WalkingFK::initialize] Unable to get the string from searchable."; return false; } - m_frameLeftHandIndex = m_kinDyn.model().getFrameIndex(lHandFrame); + m_frameLeftHandIndex = m_kinDyn->model().getFrameIndex(lHandFrame); if(m_frameLeftHandIndex == iDynTree::FRAME_INVALID_INDEX) { yError() << "[WalkingFK::initialize] Unable to find the frame named: " << lHandFrame; @@ -138,7 +142,7 @@ bool WalkingFK::initialize(const yarp::os::Searchable& config, yError() << "[WalkingFK::initialize] Unable to get the string from searchable."; return false; } - m_frameRightHandIndex = m_kinDyn.model().getFrameIndex(rHandFrame); + m_frameRightHandIndex = m_kinDyn->model().getFrameIndex(rHandFrame); if(m_frameRightHandIndex == iDynTree::FRAME_INVALID_INDEX) { yError() << "[WalkingFK::initialize] Unable to find the frame named: " << rHandFrame; @@ -151,7 +155,7 @@ bool WalkingFK::initialize(const yarp::os::Searchable& config, yError() << "[WalkingFK::initialize] Unable to get the string from searchable."; return false; } - m_frameHeadIndex = m_kinDyn.model().getFrameIndex(headFrame); + m_frameHeadIndex = m_kinDyn->model().getFrameIndex(headFrame); if(m_frameHeadIndex == iDynTree::FRAME_INVALID_INDEX) { yError() << "[WalkingFK::initialize] Unable to find the frame named: " << headFrame; @@ -164,7 +168,7 @@ bool WalkingFK::initialize(const yarp::os::Searchable& config, yError() << "[WalkingFK::initialize] Unable to get the string from searchable."; return false; } - m_frameRootIndex = m_kinDyn.model().getFrameIndex(rootFrame); + m_frameRootIndex = m_kinDyn->model().getFrameIndex(rootFrame); if(m_frameRootIndex == iDynTree::FRAME_INVALID_INDEX) { yError() << "[WalkingFK::initialize] Unable to find the frame named: " << rootFrame; @@ -177,7 +181,7 @@ bool WalkingFK::initialize(const yarp::os::Searchable& config, yError() << "[WalkingFK::initialize] Unable to get the string from searchable."; return false; } - m_frameNeckIndex = m_kinDyn.model().getFrameIndex(torsoFrame); + m_frameNeckIndex = m_kinDyn->model().getFrameIndex(torsoFrame); if(m_frameNeckIndex == iDynTree::FRAME_INVALID_INDEX) { yError() << "[WalkingFK::initialize] Unable to find the frame named: " << torsoFrame; @@ -213,7 +217,7 @@ bool WalkingFK::initialize(const yarp::os::Searchable& config, } // in this specific case the base is always the root link - if(!m_kinDyn.setFloatingBase(m_baseFrames["root"].first)) + if(!m_kinDyn->setFloatingBase(m_baseFrames["root"].first)) { yError() << "[initialize] Unable to set the floating base"; return false; @@ -303,7 +307,7 @@ bool WalkingFK::evaluateWorldToBaseTransformation(const iDynTree::Transform& lef { auto& base = m_baseFrames["leftFoot"]; m_worldToBaseTransform = leftFootTransform * base.second; - if(!m_kinDyn.setFloatingBase(base.first)) + if(!m_kinDyn->setFloatingBase(base.first)) { yError() << "[evaluateWorldToBaseTransformation] Error while setting the floating " << "base on link " << base.first; @@ -320,7 +324,7 @@ bool WalkingFK::evaluateWorldToBaseTransformation(const iDynTree::Transform& lef { auto base = m_baseFrames["rightFoot"]; m_worldToBaseTransform = rightFootTransform * base.second; - if(!m_kinDyn.setFloatingBase(base.first)) + if(!m_kinDyn->setFloatingBase(base.first)) { yError() << "[WalkingFK::evaluateWorldToBaseTransformation] Error while setting the floating " << "base on link " << base.first; @@ -344,7 +348,7 @@ bool WalkingFK::setInternalRobotState(const iDynTree::VectorDynSize& positionFee gravity.zero(); gravity(2) = -9.81; - if(!m_kinDyn.setRobotState(m_worldToBaseTransform, positionFeedbackInRadians, + if(!m_kinDyn->setRobotState(m_worldToBaseTransform, positionFeedbackInRadians, m_baseTwist, velocityFeedbackInRadians, gravity)) { @@ -363,8 +367,8 @@ void WalkingFK::evaluateCoM() if(m_comEvaluated) return; - m_comPosition = m_kinDyn.getCenterOfMassPosition(); - m_comVelocity = m_kinDyn.getCenterOfMassVelocity(); + m_comPosition = m_kinDyn->getCenterOfMassPosition(); + m_comVelocity = m_kinDyn->getCenterOfMassVelocity(); yarp::sig::Vector temp; temp.resize(3); @@ -442,7 +446,7 @@ bool WalkingFK::setBaseOnTheFly() auto base = m_baseFrames["leftFoot"]; m_worldToBaseTransform = base.second; - if(!m_kinDyn.setFloatingBase(base.first)) + if(!m_kinDyn->setFloatingBase(base.first)) { yError() << "[setBaseOnTheFly] Error while setting the floating base on link " << base.first; @@ -454,85 +458,90 @@ bool WalkingFK::setBaseOnTheFly() iDynTree::Transform WalkingFK::getLeftFootToWorldTransform() { - return m_kinDyn.getWorldTransform(m_frameLeftIndex); + return m_kinDyn->getWorldTransform(m_frameLeftIndex); } iDynTree::Transform WalkingFK::getRightFootToWorldTransform() { - return m_kinDyn.getWorldTransform(m_frameRightIndex); + return m_kinDyn->getWorldTransform(m_frameRightIndex); } iDynTree::Transform WalkingFK::getLeftHandToWorldTransform() { - return m_kinDyn.getWorldTransform(m_frameLeftHandIndex); + return m_kinDyn->getWorldTransform(m_frameLeftHandIndex); } iDynTree::Transform WalkingFK::getRightHandToWorldTransform() { - return m_kinDyn.getWorldTransform(m_frameRightHandIndex); + return m_kinDyn->getWorldTransform(m_frameRightHandIndex); } iDynTree::Transform WalkingFK::getHeadToWorldTransform() { - return m_kinDyn.getWorldTransform(m_frameHeadIndex); + return m_kinDyn->getWorldTransform(m_frameHeadIndex); } iDynTree::Transform WalkingFK::getRootLinkToWorldTransform() { - return m_kinDyn.getWorldTransform(m_frameRootIndex); + return m_kinDyn->getWorldTransform(m_frameRootIndex); } iDynTree::Twist WalkingFK::getRootLinkVelocity() { - return m_kinDyn.getFrameVel(m_frameRootIndex); + return m_kinDyn->getFrameVel(m_frameRootIndex); } iDynTree::Rotation WalkingFK::getNeckOrientation() { - return m_kinDyn.getWorldTransform(m_frameNeckIndex).getRotation(); + return m_kinDyn->getWorldTransform(m_frameNeckIndex).getRotation(); } bool WalkingFK::getLeftFootJacobian(iDynTree::MatrixDynSize &jacobian) { - return m_kinDyn.getFrameFreeFloatingJacobian(m_frameLeftIndex, jacobian); + return m_kinDyn->getFrameFreeFloatingJacobian(m_frameLeftIndex, jacobian); } bool WalkingFK::getRightFootJacobian(iDynTree::MatrixDynSize &jacobian) { - return m_kinDyn.getFrameFreeFloatingJacobian(m_frameRightIndex, jacobian); + return m_kinDyn->getFrameFreeFloatingJacobian(m_frameRightIndex, jacobian); } bool WalkingFK::getRightHandJacobian(iDynTree::MatrixDynSize &jacobian) { - return m_kinDyn.getFrameFreeFloatingJacobian(m_frameRightHandIndex, jacobian); + return m_kinDyn->getFrameFreeFloatingJacobian(m_frameRightHandIndex, jacobian); } bool WalkingFK::getLeftHandJacobian(iDynTree::MatrixDynSize &jacobian) { - return m_kinDyn.getFrameFreeFloatingJacobian(m_frameLeftHandIndex, jacobian); + return m_kinDyn->getFrameFreeFloatingJacobian(m_frameLeftHandIndex, jacobian); } bool WalkingFK::getRootLinkJacobian(iDynTree::MatrixDynSize &jacobian) { - return m_kinDyn.getFrameFreeFloatingJacobian(m_frameRootIndex, jacobian); + return m_kinDyn->getFrameFreeFloatingJacobian(m_frameRootIndex, jacobian); } bool WalkingFK::getNeckJacobian(iDynTree::MatrixDynSize &jacobian) { - return m_kinDyn.getFrameFreeFloatingJacobian(m_frameNeckIndex, jacobian); + return m_kinDyn->getFrameFreeFloatingJacobian(m_frameNeckIndex, jacobian); } bool WalkingFK::getCoMJacobian(iDynTree::MatrixDynSize &jacobian) { - return m_kinDyn.getCenterOfMassJacobian(jacobian); + return m_kinDyn->getCenterOfMassJacobian(jacobian); } const iDynTree::VectorDynSize& WalkingFK::getJointPos() { - bool ok = m_kinDyn.getJointPos(m_jointPositions); + bool ok = m_kinDyn->getJointPos(m_jointPositions); assert(ok); return m_jointPositions; } + +std::shared_ptr WalkingFK::getKinDyn() +{ + return m_kinDyn; +} diff --git a/src/RetargetingHelper/CMakeLists.txt b/src/RetargetingHelper/CMakeLists.txt index a82b5b9c..e55d7ea7 100644 --- a/src/RetargetingHelper/CMakeLists.txt +++ b/src/RetargetingHelper/CMakeLists.txt @@ -6,5 +6,5 @@ add_walking_controllers_library( NAME RetargetingHelper SOURCES src/Helper.cpp PUBLIC_HEADERS include/WalkingControllers/RetargetingHelper/Helper.h - PUBLIC_LINK_LIBRARIES WalkingControllers::YarpUtilities WalkingControllers::KinDynWrapper ${iDynTree_LIBRARIES} ctrlLib + PUBLIC_LINK_LIBRARIES WalkingControllers::HumanState WalkingControllers::YarpUtilities WalkingControllers::KinDynWrapper ${iDynTree_LIBRARIES} ctrlLib PRIVATE_LINK_LIBRARIES Eigen3::Eigen) diff --git a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h index dc899cb6..9ffc7a97 100644 --- a/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h +++ b/src/RetargetingHelper/include/WalkingControllers/RetargetingHelper/Helper.h @@ -10,12 +10,13 @@ #define WALKING_CONTROLLERS_RETARGETING_HELPER_HELPER_H // std +#include #include #include // iDyntree -#include #include +#include #include // yarp @@ -27,6 +28,7 @@ #include #include +#include namespace WalkingControllers { @@ -34,158 +36,181 @@ namespace WalkingControllers /** * Client for the retargeting application */ - class RetargetingClient +class RetargetingClient +{ +public: + enum class Phase + { + /** In this phase the smoothing time of the minimum jerk trajectory is + increased. This will guarantee a smoother transition between the + initial joint configuration and the desired joint configuration. */ + Approaching, + Stance, + Walking + }; + +private: + template struct RetargetingElement { - public: - enum class Phase - { - /** In this phase the smoothing time of the minimum jerk trajectory is - increased. This will guarantee a smoother transition between the - initial joint configuration and the desired joint configuration. */ - approacing, - stance, - walking - }; - - private: - - template - struct RetargetingElement - { - yarp::sig::Vector yarpReadBuffer; - std::unique_ptr smoother; - yarp::os::BufferedPort port; - double smoothingTimeInApproaching; - double smoothingTimeInWalking; - Data data; - }; - - bool m_useHandRetargeting; /**< True if the hand retargeting is used */ - bool m_useVirtualizer; /**< True if the virtualizer is used */ - bool m_useJointRetargeting; /**< True if the joint retargeting is used */ - bool m_useCoMHeightRetargeting; /**< True if the com retargeting is used */ - - RetargetingElement m_leftHand; /**< Left hand retargeting element */ - RetargetingElement m_rightHand; /**< Right hand retargeting element */ - - /** Offset of the CoM Height coming from the user. It is required given the different size - * between the human and the robot */ - double m_comHeightInputOffset; - - /** Desired value of the CoM height used during walking. The simplified model used for - * the locomotion is based on the assumption of a constant CoM height */ - double m_comConstantHeight; - - /** Factor required to scale the human CoM displacement to a desired robot CoM displacement */ - double m_comHeightScalingFactor; - - struct KinematicState - { - double position; - double velocity; - }; - RetargetingElement m_comHeight; - - std::vector m_retargetJointsIndex; /**< Vector containing the indices of the retargeted joints. */ - RetargetingElement m_jointRetargeting; /**< Joint retargeting element */ - - yarp::os::BufferedPort m_robotOrientationPort; /**< Average orientation of the robot.*/ - - Phase m_phase{Phase::approacing}; - double m_startingApproachingPhaseTime; /**< Initial time of the approaching phase (seconds) */ - double m_approachPhaseDuration; /**< Duration of the approaching phase (seconds) */ - - /** - * Convert a yarp vector containing position + rpy into an iDynTree homogeneous transform - * @param vector a 6d yarp vector - * @param transform an iDyntree homogeneous transformation - */ - void convertYarpVectorPoseIntoTransform(const yarp::sig::Vector& vector, - iDynTree::Transform& transform); - - /** - * Terminate the approaching phase - */ - void stopApproachingPhase(); - - public: - - /** - * Initialize the client - * @param config configuration parameters - * @param name name of the module - * @param period period of the module - * @param controlledJointsName name of the controlled joints - * @return true/false in case of success/failure - */ - bool initialize(const yarp::os::Searchable &config, - const std::string &name, - const double &period, - const std::vector& controlledJointNames); - - /** - * Reset the client - * @param kinDynWrapper a wrapper of KinDynComputations useful to get some - * informations of the current status of the robot - * @return true/false in case of success/failure - */ - bool reset(WalkingFK& kinDynWrapper); - - /** - * Close the client - */ - void close(); - - /** - * Get the feedback of the server - */ - void getFeedback(); - - /** - * Get the homogeneous transform of the left hand w.r.t. the head frame head_T_leftHand - */ - const iDynTree::Transform& leftHandTransform() const; - - /** - * Get the homogeneous transform of the right hand w.r.t. the head frame head_T_rightHand - */ - const iDynTree::Transform& rightHandTransform() const; - - /** - * Get the value of the retarget joints - */ - const iDynTree::VectorDynSize& jointValues() const; - - /** - * Get the CoM height - * @return height of the CoM - */ - double comHeight() const; - - /** - * Get the CoM height velocity - * @return height velocity of the CoM - */ - double comHeightVelocity() const; - - /** - * Get the homogeneous transform of the right hand w.r.t. the head frame head_T_rightHand - */ - void setRobotBaseOrientation(const iDynTree::Rotation& rotation); - - - void setPhase(Phase phase); - - /** - * Start the approaching phase - */ - void startApproachingPhase(); - - /** - * Check if the approaching phase is running - * @return true if the approaching phase is running - */ - bool isApproachingPhase() const; + PortData yarpReadBuffer; + yarp::os::BufferedPort port; }; + + struct Smoother + { + std::unique_ptr smoother; + yarp::sig::Vector yarpBuffer; + double smoothingTimeInApproaching; + double smoothingTimeInWalking; + }; + + template struct KinematicState + { + Smoother smoother; + T position; + T velocity; + }; + + struct HDERetargeting : public RetargetingElement + { + KinematicState joints; + KinematicState com; + }; + + struct HandRetargeting : public RetargetingElement + { + Smoother smoother; + iDynTree::Transform transform; + }; + + bool m_useHandRetargeting; /**< True if the hand retargeting is used */ + bool m_useVirtualizer; /**< True if the virtualizer is used */ + bool m_useJointRetargeting; /**< True if the joint retargeting is used */ + bool m_useCoMHeightRetargeting; /**< True if the com retargeting is used */ + + HandRetargeting m_leftHand; /**< Left hand retargeting element */ + HandRetargeting m_rightHand; /**< Right hand retargeting element */ + + /** Offset of the CoM Height coming from the user. It is required given the different size + * between the human and the robot */ + double m_comHeightInputOffset; + + /** Desired value of the CoM height used during walking. The simplified model used for + * the locomotion is based on the assumption of a constant CoM height */ + double m_comConstantHeight; + + /** Factor required to scale the human CoM displacement to a desired robot CoM displacement */ + double m_comHeightScalingFactor; + + /** Mapping between the retarget joints and the controlled. */ + std::unordered_map m_retargetedJointsToControlJoints; + + /** Mapping between the retarget joints and the HDE. */ + std::unordered_map m_retargetedJointsToHDEJoints; + + HDERetargeting m_hdeRetargeting; /**< Joint retargeting element */ + + yarp::os::BufferedPort m_robotOrientationPort; /**< Average orientation of + the robot.*/ + + Phase m_phase{Phase::Approaching}; + double m_startingApproachingPhaseTime; /**< Initial time of the approaching phase (seconds) */ + double m_approachPhaseDuration; /**< Duration of the approaching phase (seconds) */ + + /** + * Convert a yarp vector containing position + rpy into an iDynTree homogeneous transform + * @param vector a 6d yarp vector + * @param transform an iDyntree homogeneous transformation + */ + void convertYarpVectorPoseIntoTransform(const yarp::sig::Vector& vector, + iDynTree::Transform& transform); + + /** + * Terminate the approaching phase + */ + void stopApproachingPhase(); + +public: + /** + * Initialize the client + * @param config configuration parameters + * @param name name of the module + * @param period period of the module + * @param controlledJointsName name of the controlled joints + * @return true/false in case of success/failure + */ + bool initialize(const yarp::os::Searchable& config, + const std::string& name, + const double& period, + const std::vector& controlledJointNames); + + /** + * Reset the client + * @param kinDynWrapper a wrapper of KinDynComputations useful to get some + * informations of the current status of the robot + * @return true/false in case of success/failure + */ + bool reset(WalkingFK& kinDynWrapper); + + /** + * Close the client + */ + void close(); + + /** + * Get the feedback of the server + */ + bool getFeedback(); + + /** + * Get the homogeneous transform of the left hand w.r.t. the head frame head_T_leftHand + */ + const iDynTree::Transform& leftHandTransform() const; + + /** + * Get the homogeneous transform of the right hand w.r.t. the head frame head_T_rightHand + */ + const iDynTree::Transform& rightHandTransform() const; + + /** + * Get the position of the retargeting joints + */ + const iDynTree::VectorDynSize& jointPositions() const; + + /** + * Get the velocity of the retargeting joints + */ + const iDynTree::VectorDynSize& jointVelocities() const; + + /** + * Get the CoM height + * @return height of the CoM + */ + double comHeight() const; + + /** + * Get the CoM height velocity + * @return height velocity of the CoM + */ + double comHeightVelocity() const; + + /** + * Get the homogeneous transform of the right hand w.r.t. the head frame head_T_rightHand + */ + void setRobotBaseOrientation(const iDynTree::Rotation& rotation); + + void setPhase(Phase phase); + + /** + * Start the approaching phase + */ + void startApproachingPhase(); + + /** + * Check if the approaching phase is running + * @return true if the approaching phase is running + */ + bool isApproachingPhase() const; }; +}; // namespace WalkingControllers #endif diff --git a/src/RetargetingHelper/src/Helper.cpp b/src/RetargetingHelper/src/Helper.cpp index 474d649d..63e07feb 100644 --- a/src/RetargetingHelper/src/Helper.cpp +++ b/src/RetargetingHelper/src/Helper.cpp @@ -50,8 +50,10 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, return false; } - m_jointRetargeting.data.resize(controlledJointNames.size()); - m_jointRetargeting.yarpReadBuffer.resize(controlledJointNames.size()); + m_hdeRetargeting.joints.position.resize(controlledJointNames.size()); + m_hdeRetargeting.joints.velocity.resize(controlledJointNames.size()); + m_hdeRetargeting.joints.smoother.yarpBuffer.resize(controlledJointNames.size()); + m_hdeRetargeting.com.smoother.yarpBuffer(1); if(!m_useHandRetargeting && !m_useVirtualizer && !m_useJointRetargeting && !m_useCoMHeightRetargeting) @@ -67,8 +69,6 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, } std::string portName; - double smoothingTimeApproching; - double smoothingTimeWalking; if(m_useHandRetargeting) { const yarp::os::Bottle& option = config.findGroup("HAND_RETARGETING"); @@ -84,24 +84,25 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, } hand.port.open("/" + name + portName); - - if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time_approaching", smoothingTimeApproching)) + if (!YarpUtilities::getNumberFromSearchable(option, + "smoothing_time_approaching", + hand.smoother.smoothingTimeInApproaching)) { - yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; + yError() << "[RetargetingClient::initialize] Unable to get the number from " + "searchable."; return false; } - hand.smoothingTimeInApproaching = smoothingTimeApproching; - - if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time_walking", smoothingTimeWalking)) + if (!YarpUtilities::getNumberFromSearchable(option, + "smoothing_time_walking", + hand.smoother.smoothingTimeInWalking)) { yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; return false; } - hand.smoothingTimeInWalking = smoothingTimeWalking; - hand.yarpReadBuffer.resize(6); - hand.smoother = std::make_unique(6, period, hand.smoothingTimeInApproaching); + hand.smoother.yarpBuffer.resize(6); + hand.smoother.smoother = std::make_unique(6, period, hand.smoother.smoothingTimeInApproaching); return true; }; @@ -131,36 +132,35 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, // find the indices for(const std::string& joint : retargetJointNames) { - int index = std::distance(controlledJointNames.begin(), std::find(controlledJointNames.begin(), - controlledJointNames.end(), - joint)); - m_retargetJointsIndex.push_back(index); - } - - if(!YarpUtilities::getStringFromSearchable(option, "joint_retargeting_port_name", - portName)) - { - yError() << "[RetargetingClient::initialize] Unable to get the string from searchable."; - return false; + const auto element = std::find(controlledJointNames.begin(), controlledJointNames.end(), joint); + if (element == controlledJointNames.end()) + { + yError() << "[RetargetingClient::initialize] Unable to find the joint named: " + << joint << " in the list of the controlled joints."; + return false; + } + m_retargetedJointsToControlJoints[joint] = std::distance(controlledJointNames.begin(), element); } - m_jointRetargeting.port.open("/" + name + portName); if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time_approaching", - m_jointRetargeting.smoothingTimeInApproaching)) + m_hdeRetargeting.joints.smoother.smoothingTimeInApproaching)) { yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; return false; } - if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time_walking", m_jointRetargeting.smoothingTimeInWalking)) + if (!YarpUtilities::getNumberFromSearchable(option, + "smoothing_time_walking", + m_hdeRetargeting.joints.smoother.smoothingTimeInWalking)) { yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; return false; } - m_jointRetargeting.smoother = std::make_unique(controlledJointNames.size(), period, - m_jointRetargeting.smoothingTimeInApproaching); - + m_hdeRetargeting.joints.smoother.smoother + = std::make_unique(controlledJointNames.size(), + period, + m_hdeRetargeting.joints.smoother.smoothingTimeInApproaching); } if(m_useVirtualizer) @@ -176,39 +176,48 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, m_robotOrientationPort.open("/" + name + portName); } - if(m_useCoMHeightRetargeting) + if (m_useCoMHeightRetargeting) { const yarp::os::Bottle& option = config.findGroup("COM_RETARGETING"); - m_comHeight.yarpReadBuffer.resize(1); - - if(!YarpUtilities::getStringFromSearchable(option, "com_height_retargeting_port_name", portName)) + if (!YarpUtilities::getNumberFromSearchable(option, + "smoothing_time_approaching", + m_hdeRetargeting.com.smoother.smoothingTimeInApproaching)) { - yError() << "[RetargetingClient::initialize] Unable to get the string from searchable."; + yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; return false; } - m_comHeight.port.open("/" + name + portName); - if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time_approaching", m_comHeight.smoothingTimeInApproaching)) + if (!YarpUtilities::getNumberFromSearchable(option, + "smoothing_time_walking", + m_hdeRetargeting.com.smoother.smoothingTimeInWalking)) { yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; return false; } - if(!YarpUtilities::getNumberFromSearchable(option, "smoothing_time_walking", m_comHeight.smoothingTimeInWalking)) + m_hdeRetargeting.com.smoother.smoother + = std::make_unique(1, + period, + m_hdeRetargeting.com.smoother.smoothingTimeInApproaching); + + if (!YarpUtilities::getNumberFromSearchable(option, + "com_height_scaling_factor", + m_comHeightScalingFactor)) { yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; return false; } + } - m_comHeight.smoother = std::make_unique(1, period, m_comHeight.smoothingTimeInApproaching); - - if(!YarpUtilities::getNumberFromSearchable(option, "com_height_scaling_factor", - m_comHeightScalingFactor)) + if (m_useJointRetargeting || m_useCoMHeightRetargeting) + { + if (!YarpUtilities::getStringFromSearchable(config, "hde_port_name", portName)) { - yError() << "[RetargetingClient::initialize] Unable to get the number from searchable."; + yError() << "[RetargetingClient::initialize] Unable to get the string from searchable."; return false; } + m_hdeRetargeting.port.open("/" + name + portName); } return true; @@ -216,20 +225,20 @@ bool RetargetingClient::initialize(const yarp::os::Searchable &config, bool RetargetingClient::reset(WalkingFK& kinDynWrapper) { - m_leftHand.data = kinDynWrapper.getHeadToWorldTransform().inverse() * kinDynWrapper.getLeftHandToWorldTransform(); - m_rightHand.data = kinDynWrapper.getHeadToWorldTransform().inverse() * kinDynWrapper.getRightHandToWorldTransform(); + m_leftHand.transform = kinDynWrapper.getHeadToWorldTransform().inverse() * kinDynWrapper.getLeftHandToWorldTransform(); + m_rightHand.transform = kinDynWrapper.getHeadToWorldTransform().inverse() * kinDynWrapper.getRightHandToWorldTransform(); if(m_useHandRetargeting) { auto resetHandSmoother = [](auto& hand) { iDynTree::toEigen(hand.yarpReadBuffer).template segment<3>(0) = - iDynTree::toEigen(hand.data.getPosition()); + iDynTree::toEigen(hand.transform.getPosition()); iDynTree::toEigen(hand.yarpReadBuffer).template segment<3>(3) = - iDynTree::toEigen(hand.data.getRotation().asRPY()); + iDynTree::toEigen(hand.transform.getRotation().asRPY()); - hand.smoother->init(hand.yarpReadBuffer); + hand.smoother.smoother->init(hand.yarpReadBuffer); }; resetHandSmoother(m_leftHand); @@ -237,19 +246,22 @@ bool RetargetingClient::reset(WalkingFK& kinDynWrapper) } // joint retargeting - m_jointRetargeting.data = kinDynWrapper.getJointPos(); - iDynTree::toEigen(m_jointRetargeting.yarpReadBuffer) = iDynTree::toEigen(m_jointRetargeting.data); + m_hdeRetargeting.joints.position = kinDynWrapper.getJointPos(); + m_hdeRetargeting.joints.velocity.zero(); + iDynTree::toEigen(m_hdeRetargeting.joints.smoother.yarpBuffer) = iDynTree::toEigen(m_hdeRetargeting.joints.position); if (m_useJointRetargeting) - m_jointRetargeting.smoother->init(m_jointRetargeting.yarpReadBuffer); + { + m_hdeRetargeting.joints.smoother.smoother->init(m_hdeRetargeting.joints.smoother.yarpBuffer); + } - m_comHeight.data.position = kinDynWrapper.getCoMPosition()(2); - m_comHeight.data.velocity = 0; - m_comConstantHeight = m_comHeight.data.position; + m_hdeRetargeting.com.position = kinDynWrapper.getCoMPosition()(2); + m_hdeRetargeting.com.velocity = 0; + m_comConstantHeight = m_hdeRetargeting.com.position; if(m_useCoMHeightRetargeting) { - m_comHeight.yarpReadBuffer(0) = m_comHeight.data.position; - m_comHeight.smoother->init(m_comHeight.yarpReadBuffer); + m_hdeRetargeting.com.smoother.yarpBuffer(0) = m_hdeRetargeting.com.position; + m_hdeRetargeting.joints.smoother.smoother->init(m_hdeRetargeting.com.smoother.yarpBuffer); // let's read the port to reset the comHeightInput bool okCoMHeight = false; @@ -258,10 +270,10 @@ bool RetargetingClient::reset(WalkingFK& kinDynWrapper) { if(!okCoMHeight) { - auto desiredCoMHeight = m_comHeight.port.read(false); - if(desiredCoMHeight != nullptr) + auto data = m_hdeRetargeting.port.read(false); + if(data != nullptr) { - m_comHeightInputOffset = (*desiredCoMHeight)(2); + m_comHeightInputOffset = data->CoMPositionWRTGlobal.z; okCoMHeight = true; } } @@ -281,108 +293,146 @@ bool RetargetingClient::reset(WalkingFK& kinDynWrapper) return true; } -void RetargetingClient::getFeedback() +bool RetargetingClient::getFeedback() { if(m_useHandRetargeting) { - auto getHandFeedback = [this](auto& hand) + auto getHandFeedback = [this](HandRetargeting& hand) { auto desiredHandPose = hand.port.read(false); if(desiredHandPose != nullptr) - hand.yarpReadBuffer = *desiredHandPose; - - hand.smoother->computeNextValues(hand.yarpReadBuffer); - convertYarpVectorPoseIntoTransform(hand.smoother->getPos(), hand.data); + { + hand.smoother.smoother->computeNextValues(*desiredHandPose); + } + convertYarpVectorPoseIntoTransform(hand.smoother.smoother->getPos(), hand.transform); }; getHandFeedback(m_leftHand); getHandFeedback(m_rightHand); } - if(m_useJointRetargeting) + if (m_useJointRetargeting || m_useCoMHeightRetargeting) { - auto desiredJoint = m_jointRetargeting.port.read(false); - if(desiredJoint != nullptr) + const auto HDEData = m_hdeRetargeting.port.read(false); + if (HDEData != nullptr) { - for(int i =0; i < desiredJoint->size(); i++) - m_jointRetargeting.yarpReadBuffer(m_retargetJointsIndex[i]) = (*desiredJoint)(i); - } + if (m_useCoMHeightRetargeting) + { + if (m_phase == Phase::Walking) + { + m_hdeRetargeting.com.smoother.yarpBuffer(0) = m_comConstantHeight; + } else + { + const auto& desiredCoMHeight = HDEData->CoMPositionWRTGlobal.z; + m_hdeRetargeting.com.smoother.yarpBuffer(0) + = (desiredCoMHeight - m_comHeightInputOffset) * m_comHeightScalingFactor + + m_comConstantHeight; + } + } - m_jointRetargeting.smoother->computeNextValues(m_jointRetargeting.yarpReadBuffer); - iDynTree::toEigen(m_jointRetargeting.data) = iDynTree::toEigen(m_jointRetargeting.smoother->getPos()); - } + if (m_useJointRetargeting) + { + // the first time we should define the map between the retargeted joints and the + // data coming from HDE + if (m_retargetedJointsToHDEJoints.empty()) + { + const auto& HDEJointNames = HDEData->jointNames; + for (const auto& [joint, index] : m_retargetedJointsToControlJoints) + { + const auto element = std::find(HDEJointNames.begin(), HDEJointNames.end(), joint); + if (element == HDEJointNames.end()) + { + yError() << "[RetargetingClient::getFeedback] Unable to find the joint " + "named: " + << joint << " in the list of the HDE joints."; + return false; + } + m_retargetedJointsToHDEJoints[joint] = std::distance(HDEJointNames.begin(), element); + } + } + const auto& HDEJoints = HDEData->positions; + for (const auto& [joint, index] : m_retargetedJointsToControlJoints) + { + m_hdeRetargeting.joints.smoother.yarpBuffer(index) = HDEData->positions[m_retargetedJointsToHDEJoints[joint]]; + } + } + } + } - if(m_useCoMHeightRetargeting) + // even if the data is not arrived the minimum jerk trajectory has to be updated. This will + // generate a smoother trajectory + if (m_useCoMHeightRetargeting) { - if(m_phase == Phase::walking) - m_comHeight.yarpReadBuffer(0) = m_comConstantHeight; - else - { - auto desiredCoMHeight = m_comHeight.port.read(false); - if(desiredCoMHeight != nullptr) - - m_comHeight.yarpReadBuffer(0) = ((*desiredCoMHeight)(2) - m_comHeightInputOffset) - * m_comHeightScalingFactor + m_comConstantHeight; - } + m_hdeRetargeting.com.smoother.smoother->computeNextValues( + m_hdeRetargeting.com.smoother.yarpBuffer); + m_hdeRetargeting.com.position = m_hdeRetargeting.com.smoother.smoother->getPos()(0); + m_hdeRetargeting.com.velocity = m_hdeRetargeting.com.smoother.smoother->getVel()(0); + } - m_comHeight.smoother->computeNextValues(m_comHeight.yarpReadBuffer); - m_comHeight.data.position = m_comHeight.smoother->getPos()(0); - m_comHeight.data.velocity = m_comHeight.smoother->getVel()(0); + if (m_useJointRetargeting) + { + m_hdeRetargeting.joints.smoother.smoother->computeNextValues( + m_hdeRetargeting.joints.smoother.yarpBuffer); + iDynTree::toEigen(m_hdeRetargeting.joints.position) + = iDynTree::toEigen(m_hdeRetargeting.joints.smoother.smoother->getPos()); + iDynTree::toEigen(m_hdeRetargeting.joints.velocity) + = iDynTree::toEigen(m_hdeRetargeting.joints.smoother.smoother->getVel()); } // check if the approaching phase is finished - if(m_phase == Phase::approacing) + if(m_phase == Phase::Approaching) { double now = yarp::os::Time::now(); if(now - m_startingApproachingPhaseTime > m_approachPhaseDuration) stopApproachingPhase(); } - return; + return true; } const iDynTree::Transform& RetargetingClient::leftHandTransform() const { - return m_leftHand.data; + return m_leftHand.transform; } const iDynTree::Transform& RetargetingClient::rightHandTransform() const { - return m_rightHand.data; + return m_rightHand.transform; +} + +const iDynTree::VectorDynSize& RetargetingClient::jointPositions() const +{ + return m_hdeRetargeting.joints.position; } -const iDynTree::VectorDynSize& RetargetingClient::jointValues() const +const iDynTree::VectorDynSize& RetargetingClient::jointVelocities() const { - return m_jointRetargeting.data; + return m_hdeRetargeting.joints.velocity; } double RetargetingClient::comHeight() const { - return m_comHeight.data.position; + return m_hdeRetargeting.com.position; } double RetargetingClient::comHeightVelocity() const { - return m_comHeight.data.velocity; + return m_hdeRetargeting.com.velocity; } void RetargetingClient::close() { - if(m_useHandRetargeting) + if (m_useHandRetargeting) { m_leftHand.port.close(); m_rightHand.port.close(); } - if(m_useJointRetargeting) - m_jointRetargeting.port.close(); - - if(m_useCoMHeightRetargeting) - m_comHeight.port.close(); - - if(m_useVirtualizer) - m_robotOrientationPort.close(); + if (m_useJointRetargeting || m_useCoMHeightRetargeting) + { + m_hdeRetargeting.port.close(); + } } void RetargetingClient::setRobotBaseOrientation(const iDynTree::Rotation& rotation) @@ -398,15 +448,15 @@ void RetargetingClient::setRobotBaseOrientation(const iDynTree::Rotation& rotati void RetargetingClient::setPhase(Phase phase) { - if(phase == Phase::approacing) + if (phase == Phase::Approaching) { startApproachingPhase(); } - if(m_phase == Phase::approacing && phase == Phase::walking) + if (m_phase == Phase::Approaching && phase == Phase::Walking) stopApproachingPhase(); - if(m_phase == Phase::approacing && phase == Phase::stance) + if (m_phase == Phase::Approaching && phase == Phase::Stance) stopApproachingPhase(); m_phase = phase; @@ -414,23 +464,25 @@ void RetargetingClient::setPhase(Phase phase) void RetargetingClient::stopApproachingPhase() { - if(m_useHandRetargeting) + if (m_useHandRetargeting) { - m_leftHand.smoother->setT(m_leftHand.smoothingTimeInWalking); - m_rightHand.smoother->setT(m_rightHand.smoothingTimeInWalking); + m_leftHand.smoother.smoother->setT(m_leftHand.smoother.smoothingTimeInWalking); + m_rightHand.smoother.smoother->setT(m_rightHand.smoother.smoothingTimeInWalking); } - if(m_useJointRetargeting) + if (m_useJointRetargeting) { - m_jointRetargeting.smoother->setT(m_jointRetargeting.smoothingTimeInWalking); + m_hdeRetargeting.joints.smoother.smoother->setT( + m_hdeRetargeting.joints.smoother.smoothingTimeInWalking); } - if(m_useCoMHeightRetargeting) + if (m_useCoMHeightRetargeting) { - m_comHeight.smoother->setT(m_comHeight.smoothingTimeInWalking); + m_hdeRetargeting.com.smoother.smoother->setT( + m_hdeRetargeting.com.smoother.smoothingTimeInWalking); } - m_phase = Phase::stance; + m_phase = Phase::Stance; } void RetargetingClient::startApproachingPhase() @@ -443,23 +495,25 @@ void RetargetingClient::startApproachingPhase() if(m_useHandRetargeting) { - m_leftHand.smoother->setT(m_leftHand.smoothingTimeInApproaching); - m_rightHand.smoother->setT(m_rightHand.smoothingTimeInApproaching); + m_leftHand.smoother.smoother->setT(m_leftHand.smoother.smoothingTimeInApproaching); + m_rightHand.smoother.smoother->setT(m_rightHand.smoother.smoothingTimeInApproaching); } - if(m_useJointRetargeting) + if (m_useJointRetargeting) { - m_jointRetargeting.smoother->setT(m_jointRetargeting.smoothingTimeInApproaching); + m_hdeRetargeting.joints.smoother.smoother->setT( + m_hdeRetargeting.joints.smoother.smoothingTimeInApproaching); } - if(m_useCoMHeightRetargeting) + if (m_useCoMHeightRetargeting) { - m_comHeight.smoother->setT(m_comHeight.smoothingTimeInApproaching); + m_hdeRetargeting.com.smoother.smoother->setT( + m_hdeRetargeting.com.smoother.smoothingTimeInApproaching); } } bool RetargetingClient::isApproachingPhase() const { - return m_phase == Phase::approacing; + return m_phase == Phase::Approaching; } diff --git a/src/WalkingModule/CMakeLists.txt b/src/WalkingModule/CMakeLists.txt index 6bf0ba2d..a01fbf76 100644 --- a/src/WalkingModule/CMakeLists.txt +++ b/src/WalkingModule/CMakeLists.txt @@ -10,6 +10,6 @@ add_walking_controllers_application( NAME WalkingModule SOURCES src/main.cpp src/Module.cpp ${WalkingModule_THRIFT_GEN_FILES} HEADERS include/WalkingControllers/WalkingModule/Module.h - LINK_LIBRARIES WalkingControllers::YarpUtilities WalkingControllers::iDynTreeUtilities WalkingControllers::StdUtilities WalkingControllers::TimeProfiler WalkingControllers::RobotInterface WalkingControllers::KinDynWrapper WalkingControllers::TrajectoryPlanner WalkingControllers::SimplifiedModelControllers WalkingControllers::WholeBodyControllers WalkingControllers::RetargetingHelper BipedalLocomotion::VectorsCollection ctrlLib + LINK_LIBRARIES WalkingControllers::YarpUtilities WalkingControllers::iDynTreeUtilities WalkingControllers::StdUtilities WalkingControllers::TimeProfiler WalkingControllers::RobotInterface WalkingControllers::KinDynWrapper WalkingControllers::TrajectoryPlanner WalkingControllers::SimplifiedModelControllers WalkingControllers::WholeBodyControllers WalkingControllers::RetargetingHelper BipedalLocomotion::VectorsCollection BipedalLocomotion::ParametersHandlerYarpImplementation ctrlLib SUBDIRECTORIES app) diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini new file mode 100644 index 00000000..5f411583 --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini @@ -0,0 +1,29 @@ +[IK] +robot_velocity_variable_name "robot_velocity" + +[COM_TASK] +robot_velocity_variable_name "robot_velocity" +kp_linear 1.0 +mask (true, true, false) + +[ROOT_TASK] +robot_velocity_variable_name "robot_velocity" +frame_name "root_link" +kp_linear 1.0 +mask (false, false, true) + + +[RIGHT_FOOT_TASK] +robot_velocity_variable_name "robot_velocity" +frame_name "r_sole" +kp_linear 7.0 +kp_angular 5.0 + +[LEFT_FOOT_TASK] +robot_velocity_variable_name "robot_velocity" +frame_name "l_sole" +kp_linear 7.0 +kp_angular 5.0 + +[include TORSO_TASK "./tasks/torso.ini"] +[include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"] diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/tasks/regularization.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/tasks/regularization.ini new file mode 100644 index 00000000..4505b5a1 --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/tasks/regularization.ini @@ -0,0 +1,26 @@ +robot_velocity_variable_name "robot_velocity" +kp (5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) + +states ("STANCE", "WALKING") +sampling_time 0.01 +settling_time 0.5 + +[STANCE] +name "stance" +weight (1.0, 1.0, 1.0, + 2.0, 2.0, 2.0, 2.0, + 2.0, 2.0, 2.0, 2.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + +[WALKING] +name "walking" +weight (1.0, 1.0, 1.0, + 2.0, 2.0, 2.0, 2.0, + 2.0, 2.0, 2.0, 2.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/tasks/torso.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/tasks/torso.ini new file mode 100644 index 00000000..7231b689 --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking/joypad_control/tasks/torso.ini @@ -0,0 +1,16 @@ +robot_velocity_variable_name "robot_velocity" +frame_name "chest" +kp_angular 5.0 + + +states ("STANCE", "WALKING") +sampling_time 0.01 +settling_time 0.5 + +[STANCE] +name "stance" +weight (5.0, 5.0, 5.0) + +[WALKING] +name "walking" +weight (5.0, 5.0, 5.0) diff --git a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_with_joypad.ini index 9ddc3071..8c4b242e 100644 --- a/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/iCubGazeboV3/dcm_walking_with_joypad.ini @@ -8,6 +8,8 @@ use_QP-IK 1 # solve QP-IK. In this case qpOASES will be used use_osqp 1 +use_BLF-IK 1 + # remove this line if you don't want to save data of the experiment dump_data 0 @@ -44,7 +46,7 @@ com_height 0.535 # sampling time sampling_time 0.01 # Specify the frame to use to control the robot height. Currently, we support only the following options: com, root_link -height_reference_frame com +height_reference_frame root_link # include robot control parameters [include ROBOT_CONTROL "./dcm_walking/joypad_control/robotControl.ini"] @@ -67,8 +69,12 @@ height_reference_frame com # include inverse kinematcs parameters [include INVERSE_KINEMATICS_SOLVER "./dcm_walking/joypad_control/inverseKinematics.ini"] +# # include qp inverse kinematcs parameters +# [include INVERSE_KINEMATICS_QP_SOLVER "./dcm_walking/joypad_control/qpInverseKinematics.ini"] + # include qp inverse kinematcs parameters -[include INVERSE_KINEMATICS_QP_SOLVER "./dcm_walking/joypad_control/qpInverseKinematics.ini"] +[include INVERSE_KINEMATICS_BLF_QP_SOLVER "./dcm_walking/joypad_control/qpInverseKinematicsBlf.ini"] + # include inverse kinematcs parameters [include FORWARD_KINEMATICS_SOLVER "./dcm_walking/common/forwardKinematics.ini"] diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/pidParams.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/pidParams.ini index fe6023c1..8891b89d 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/pidParams.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/pidParams.ini @@ -29,9 +29,9 @@ r_hip_yaw 10000.0 2000.0 0.0 l_hip_yaw -7000.0 -2000.0 0.0 #r_shoulder_pitch -10000.0 -10000.0 0.0 #r_shoulder_roll -10000.0 -10000.0 0.0 -#r_shoulder_yaw -7500.0 -7500.0 0.0 +r_shoulder_yaw -3000.0 -1500.0 0.0 #r_elbow 4000.0 4000.0 0.0 #l_shoulder_pitch 10000.0 10000.0 0.0 #l_shoulder_roll 10000.0 10000.0 0.0 -#l_shoulder_yaw 7500.0 7500.0 0.0 +l_shoulder_yaw 3000.0 1500.0 0.0 #l_elbow -4000.0 -4000.0 0.0 diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini index d6fbcf5c..810708c7 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini @@ -1,4 +1,5 @@ approaching_phase_duration 4.0 +hde_port_name /humanState:i [HAND_RETARGETING] @@ -13,16 +14,13 @@ retargeting_joint_list ("neck_pitch", "neck_roll", "neck_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", "l_wrist_pitch", "l_wrist_yaw", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw") - -joint_retargeting_port_name /jointPosition:i smoothing_time_approaching 2.0 -smoothing_time_walking 0.2 +smoothing_time_walking 0.03 [VIRTUALIZER] robot_orientation_port_name /torsoYaw:o [COM_RETARGETING] -com_height_retargeting_port_name /CoM:i smoothing_time_approaching 2.0 smoothing_time_walking 1.0 com_height_scaling_factor 0.5 diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini new file mode 100644 index 00000000..b16519e4 --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini @@ -0,0 +1,31 @@ +use_feedforward_term_for_joint_retargeting false + +[IK] +robot_velocity_variable_name "robot_velocity" + +[COM_TASK] +robot_velocity_variable_name "robot_velocity" +kp_linear 2.0 +mask (true, true, false) + +[ROOT_TASK] +robot_velocity_variable_name "robot_velocity" +frame_name "root_link" +kp_linear 0.5 +mask (false, false, true) + +[RIGHT_FOOT_TASK] +robot_velocity_variable_name "robot_velocity" +frame_name "r_sole" +kp_linear 7.0 +kp_angular 5.0 + +[LEFT_FOOT_TASK] +robot_velocity_variable_name "robot_velocity" +frame_name "l_sole" +kp_linear 7.0 +kp_angular 5.0 + +[include TORSO_TASK "./tasks/torso.ini"] +[include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"] +[include JOINT_RETARGETING_TASK "./tasks/retargeting.ini"] diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini new file mode 100644 index 00000000..d6fffcac --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini @@ -0,0 +1,29 @@ +robot_velocity_variable_name "robot_velocity" +kp (5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0 + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) + +states ("STANCE", "WALKING") +sampling_time 0.01 +settling_time 4.5 + +[STANCE] +name "stance" +weight (0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + +[WALKING] +name "walking" +weight (0.0, 0.0, 0.0, + 1.0, 1.0, 1.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini new file mode 100644 index 00000000..903728e5 --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini @@ -0,0 +1,29 @@ +robot_velocity_variable_name "robot_velocity" +kp (5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0 + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) + +states ("STANCE", "WALKING") +sampling_time 0.01 +settling_time 4.5 + +[STANCE] +name "stance" +weight (2.0, 2.0, 2.0, + 2.0, 2.0, 2.0, + 2.0, 2.0, 2.0, 2.0, 2.0, 5.0, 5.0, + 2.0, 2.0, 2.0, 2.0, 2.0, 5.0, 5.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + +[WALKING] +name "walking" +weight (2.0, 2.0, 2.0, + 1.0, 1.0, 1.0, + 2.0, 2.0, 2.0, 2.0, 2.0, 5.0, 5.0, + 2.0, 2.0, 2.0, 2.0, 2.0, 5.0, 5.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini new file mode 100644 index 00000000..e69638dd --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/iFeel_joint_retargeting/tasks/torso.ini @@ -0,0 +1,16 @@ +robot_velocity_variable_name "robot_velocity" +frame_name "chest" +kp_angular 5.0 + + +states ("STANCE", "WALKING") +sampling_time 0.01 +settling_time 4.5 + +[STANCE] +name "stance" +weight (0.0, 0.0, 0.0) + +[WALKING] +name "walking" +weight (5.0, 5.0, 5.0) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini new file mode 100644 index 00000000..98b44046 --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/qpInverseKinematicsBlf.ini @@ -0,0 +1,28 @@ +[IK] +robot_velocity_variable_name "robot_velocity" + +[COM_TASK] +robot_velocity_variable_name "robot_velocity" +kp_linear 2.0 +mask (true, true, false) + +[ROOT_TASK] +robot_velocity_variable_name "robot_velocity" +frame_name "root_link" +kp_linear 0.5 +mask (false, false, true) + +[RIGHT_FOOT_TASK] +robot_velocity_variable_name "robot_velocity" +frame_name "r_sole" +kp_linear 7.0 +kp_angular 5.0 + +[LEFT_FOOT_TASK] +robot_velocity_variable_name "robot_velocity" +frame_name "l_sole" +kp_linear 7.0 +kp_angular 5.0 + +[include TORSO_TASK "./tasks/torso.ini"] +[include JOINT_REGULARIZATION_TASK "./tasks/regularization.ini"] diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/regularization.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/regularization.ini new file mode 100644 index 00000000..87c2081f --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/regularization.ini @@ -0,0 +1,26 @@ +robot_velocity_variable_name "robot_velocity" +kp (5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0, + 5.0, 5.0, 5.0, 5.0, 5.0, 5.0) + +states ("stance", "walking") +sampling_time 0.01 +settling_time 0.5 + +[stance] +name "stance" +weight (1.0, 1.0, 1.0, + 2.0, 2.0, 2.0, 2.0, + 2.0, 2.0, 2.0, 2.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + +[walking] +name "walking" +weight (1.0, 1.0, 1.0, + 2.0, 2.0, 2.0, 2.0, + 2.0, 2.0, 2.0, 2.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/torso.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/torso.ini new file mode 100644 index 00000000..8258c0af --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/joypad_control/tasks/torso.ini @@ -0,0 +1,16 @@ +robot_velocity_variable_name "robot_velocity" +frame_name "chest" +kp_angular 5.0 + + +states ("stance", "walking") +sampling_time 0.01 +settling_time 0.5 + +[stance] +name "stance" +weight (5.0, 5.0, 5.0) + +[walking] +name "walking" +weight (5.0, 5.0, 5.0) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini index f14b4c0d..f2b5d7e4 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_iFeel_joint_retargeting.ini @@ -8,6 +8,8 @@ use_QP-IK 1 # solve QP-IK. In this case qpOASES will be used use_osqp 1 +use_BLF-IK 1 + # remove this line if you don't want to save data of the experiment dump_data 1 @@ -73,7 +75,10 @@ height_reference_frame root_link [include INVERSE_KINEMATICS_SOLVER "./dcm_walking/iFeel_joint_retargeting/inverseKinematics.ini"] # include qp inverse kinematcs parameters -[include INVERSE_KINEMATICS_QP_SOLVER "./dcm_walking/iFeel_joint_retargeting/qpInverseKinematics.ini"] +# [include INVERSE_KINEMATICS_QP_SOLVER "./dcm_walking/iFeel_joint_retargeting/qpInverseKinematics.ini"] + +# include qp inverse kinematcs parameters +[include INVERSE_KINEMATICS_BLF_QP_SOLVER "./dcm_walking/iFeel_joint_retargeting/qpInverseKinematicsBlf.ini"] # include inverse kinematcs parameters [include FORWARD_KINEMATICS_SOLVER "./dcm_walking/common/forwardKinematics.ini"] diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini index 4207c133..82912cfb 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini @@ -11,6 +11,8 @@ use_osqp 1 # remove this line if you don't want to save data of the experiment dump_data 1 +use_BLF-IK 1 + # Limit on the maximum initial velocity. This avoids the robot to jump at startup max_initial_com_vel 0.15 @@ -67,8 +69,11 @@ height_reference_frame root_link # include inverse kinematcs parameters [include INVERSE_KINEMATICS_SOLVER "./dcm_walking/joypad_control/inverseKinematics.ini"] +# # include qp inverse kinematcs parameters +# [include INVERSE_KINEMATICS_QP_SOLVER "./dcm_walking/joypad_control/qpInverseKinematics.ini"] + # include qp inverse kinematcs parameters -[include INVERSE_KINEMATICS_QP_SOLVER "./dcm_walking/joypad_control/qpInverseKinematics.ini"] +[include INVERSE_KINEMATICS_BLF_QP_SOLVER "./dcm_walking/joypad_control/qpInverseKinematicsBlf.ini"] # include inverse kinematcs parameters [include FORWARD_KINEMATICS_SOLVER "./dcm_walking/common/forwardKinematics.ini"] diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 5f921a4a..05e1a50a 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -10,6 +10,7 @@ #define WALKING_MODULE_HPP // std +#include #include #include @@ -70,6 +71,7 @@ namespace WalkingControllers bool m_useMPC; /**< True if the MPC controller is used. */ bool m_useQPIK; /**< True if the QP-IK is used. */ + bool m_useBLFIK; /**< True if the BLF-IK is used. */ bool m_useOSQP; /**< True if osqp is used to QP-IK problem. */ bool m_dumpData; /**< True if data are saved. */ bool m_firstRun; /**< True if it is the first run. */ @@ -92,6 +94,7 @@ namespace WalkingControllers std::unique_ptr m_walkingZMPController; /**< Pointer to the walking ZMP controller object. */ std::unique_ptr m_IKSolver; /**< Pointer to the inverse kinematics solver. */ std::unique_ptr m_QPIKSolver; /**< Pointer to the inverse kinematics solver. */ + std::unique_ptr m_BLFIKSolver; /**< Pointer to the integration based ik. */ std::unique_ptr m_FKSolver; /**< Pointer to the forward kinematics solver. */ std::unique_ptr m_stableDCMModel; /**< Pointer to the stable DCM dynamics. */ std::unique_ptr m_PIDHandler; /**< Pointer to the PID handler object. */ @@ -188,6 +191,10 @@ namespace WalkingControllers const iDynTree::Rotation& desiredNeckOrientation, iDynTree::VectorDynSize &output); + bool solveBLFIK(const iDynTree::Position& desiredCoMPosition, + const iDynTree::Vector3& desiredCoMVelocity, + const iDynTree::Rotation& desiredNeckOrientation, + iDynTree::VectorDynSize &output); /** * Evaluate the position of Zero momentum point. * @param zmp zero momentum point. diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 48e0ab42..a029eeac 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -16,7 +16,6 @@ #include #include - // iDynTree #include #include @@ -24,9 +23,15 @@ #include #include +// blf +#include +#include + +// walking-controllers #include #include #include +#include using namespace WalkingControllers; @@ -133,6 +138,7 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) // module name (used as prefix for opened ports) m_useMPC = rf.check("use_mpc", yarp::os::Value(false)).asBool(); m_useQPIK = rf.check("use_QP-IK", yarp::os::Value(false)).asBool(); + m_useBLFIK = rf.check("use_BLF-IK", yarp::os::Value(false)).asBool(); m_useOSQP = rf.check("use_osqp", yarp::os::Value(false)).asBool(); m_dumpData = rf.check("dump_data", yarp::os::Value(false)).asBool(); m_maxInitialCoMVelocity = rf.check("max_initial_com_vel", yarp::os::Value(1.0)).asFloat64(); @@ -296,26 +302,6 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) return false; } - if(m_useQPIK) - { - yarp::os::Bottle& inverseKinematicsQPSolverOptions = rf.findGroup("INVERSE_KINEMATICS_QP_SOLVER"); - inverseKinematicsQPSolverOptions.append(generalOptions); - if(m_useOSQP) - m_QPIKSolver = std::make_unique(); - else - m_QPIKSolver = std::make_unique(); - - if(!m_QPIKSolver->initialize(inverseKinematicsQPSolverOptions, - m_robotControlHelper->getActuatedDoFs(), - m_robotControlHelper->getVelocityLimits(), - m_robotControlHelper->getPositionUpperLimits(), - m_robotControlHelper->getPositionLowerLimits())) - { - yError() << "[WalkingModule::configure] Failed to configure the QP-IK solver (qpOASES)"; - return false; - } - } - // initialize the forward kinematics solver m_FKSolver = std::make_unique(); yarp::os::Bottle& forwardKinematicsSolverOptions = rf.findGroup("FORWARD_KINEMATICS_SOLVER"); @@ -326,6 +312,48 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) return false; } + + if (m_useQPIK) + { + if (!m_useBLFIK) + { + yarp::os::Bottle& inverseKinematicsQPSolverOptions = rf.findGroup("INVERSE_KINEMATICS_" + "QP_SOLVER"); + inverseKinematicsQPSolverOptions.append(generalOptions); + if (m_useOSQP) + m_QPIKSolver = std::make_unique(); + else + m_QPIKSolver = std::make_unique(); + + if (!m_QPIKSolver->initialize(inverseKinematicsQPSolverOptions, + m_robotControlHelper->getActuatedDoFs(), + m_robotControlHelper->getVelocityLimits(), + m_robotControlHelper->getPositionUpperLimits(), + m_robotControlHelper->getPositionLowerLimits())) + { + yError() << "[WalkingModule::configure] Failed to configure the QP-IK solver " + "(qpOASES)"; + return false; + } + } else + { + yarp::os::Bottle& inverseKinematicsQPSolverOptions = rf.findGroup("INVERSE_KINEMATICS_BLF_QP_SOLVER"); + // TODO check if this is required + inverseKinematicsQPSolverOptions.append(generalOptions); + m_BLFIKSolver= std::make_unique(); + auto paramHandler + = std::make_shared(); + paramHandler->set(inverseKinematicsQPSolverOptions); + paramHandler->setParameter("use_root_link_for_height", m_useRootLinkForHeight); + + if (!m_BLFIKSolver->initialize(paramHandler, m_FKSolver->getKinDyn())) + { + yError() << "[WalkingModule::configure] Failed to configure the blf ik solver"; + return false; + } + } + } + // initialize the linear inverted pendulum model m_stableDCMModel = std::make_unique(); if(!m_stableDCMModel->initialize(generalOptions)) @@ -486,7 +514,7 @@ bool WalkingModule::solveQPIK(const std::unique_ptr& solver, const solver->setDesiredHandsTransformation(m_FKSolver->getHeadToWorldTransform() * m_retargetingClient->leftHandTransform(), m_FKSolver->getHeadToWorldTransform() * m_retargetingClient->rightHandTransform()); - ok &= solver->setDesiredRetargetingJoint(m_retargetingClient->jointValues()); + ok &= solver->setDesiredRetargetingJoint(m_retargetingClient->jointPositions()); // set jacobians iDynTree::MatrixDynSize jacobian, comJacobian; @@ -536,6 +564,42 @@ bool WalkingModule::solveQPIK(const std::unique_ptr& solver, const return true; } + +bool WalkingModule::solveBLFIK(const iDynTree::Position& desiredCoMPosition, + const iDynTree::Vector3& desiredCoMVelocity, + const iDynTree::Rotation& desiredNeckOrientation, + iDynTree::VectorDynSize &output) +{ + const std::string phase = m_isStancePhase.front() ? "stance" : "walking"; + bool ok = m_BLFIKSolver->setPhase(phase); + ok = ok && m_BLFIKSolver->setTorsoSetPoint(desiredNeckOrientation.inverse()); + + ok = ok + && m_BLFIKSolver->setLeftFootSetPoint(m_leftTrajectory.front(), + m_leftTwistTrajectory.front()); + ok = ok + && m_BLFIKSolver->setRightFootSetPoint(m_rightTrajectory.front(), + m_rightTwistTrajectory.front()); + ok = ok && m_BLFIKSolver->setCoMSetPoint(desiredCoMPosition, desiredCoMVelocity); + ok = ok + && m_BLFIKSolver->setRetargetingJointSetPoint(m_retargetingClient->jointPositions(), + m_retargetingClient->jointVelocities()); + + if (m_useRootLinkForHeight) + { + ok = ok && m_BLFIKSolver->setRootSetPoint(desiredCoMPosition, desiredCoMVelocity); + } + + ok = ok && m_BLFIKSolver->solve(); + + if (ok) + { + output = m_BLFIKSolver->getDesiredJointVelocity(); + } + + return ok; +} + bool WalkingModule::updateModule() { std::lock_guard guard(m_mutex); @@ -721,11 +785,15 @@ bool WalkingModule::updateModule() // if the retargeting is not in the approaching phase we can set the stance/walking phase if(!m_retargetingClient->isApproachingPhase()) { - auto retargetingPhase = m_isStancePhase.front() ? RetargetingClient::Phase::stance : RetargetingClient::Phase::walking; + auto retargetingPhase = m_isStancePhase.front() ? RetargetingClient::Phase::Stance : RetargetingClient::Phase::Walking; m_retargetingClient->setPhase(retargetingPhase); } - m_retargetingClient->getFeedback(); + if (!m_retargetingClient->getFeedback()) + { + yError() << "[WalkingModule::updateModule] Unable to get the feedback from the retargeting client."; + return false; + } if(!updateFKSolver()) { @@ -878,12 +946,29 @@ bool WalkingModule::updateModule() return false; } - if(!solveQPIK(m_QPIKSolver, desiredCoMPosition, - desiredCoMVelocity, - yawRotation, m_dqDesired)) + if (!m_useBLFIK) { - yError() << "[WalkingModule::updateModule] Unable to solve the QP problem with osqp."; - return false; + if (!solveQPIK(m_QPIKSolver, + desiredCoMPosition, + desiredCoMVelocity, + yawRotation, + m_dqDesired)) + { + yError() << "[WalkingModule::updateModule] Unable to solve the QP problem with " + "osqp."; + return false; + } + } else + { + if (!solveBLFIK(desiredCoMPosition, + desiredCoMVelocity, + yawRotation, + m_dqDesired)) + { + yError() << "[WalkingModule::updateModule] Unable to solve the QP problem with " + "blf ik."; + return false; + } } iDynTree::toYarp(m_dqDesired, bufferVelocity); @@ -931,13 +1016,6 @@ bool WalkingModule::updateModule() return false; } - iDynTree::VectorDynSize errorL(6), errorR(6); - if(m_useQPIK) - { - errorR = m_QPIKSolver->getRightFootError(); - errorL = m_QPIKSolver->getLeftFootError(); - } - // send data to the logger if(m_dumpData) { @@ -1033,8 +1111,9 @@ bool WalkingModule::updateModule() // Joint data.vectors["joints_state::positions::measured"].assign(m_robotControlHelper->getJointPosition().data(), m_robotControlHelper->getJointPosition().data() + m_robotControlHelper->getJointPosition().size()); data.vectors["joints_state::positions::desired"].assign(m_qDesired.data(), m_qDesired.data() + m_qDesired.size()); - data.vectors["joints_state::positions::retargeting"].assign(m_retargetingClient->jointValues().data(), m_retargetingClient->jointValues().data() + m_retargetingClient->jointValues().size()); + data.vectors["joints_state::positions::retargeting"].assign(m_retargetingClient->jointPositions().data(), m_retargetingClient->jointPositions().data() + m_retargetingClient->jointPositions().size()); data.vectors["joints_state::velocities::measured"].assign(m_robotControlHelper->getJointVelocity().data(), m_robotControlHelper->getJointVelocity().data() + m_robotControlHelper->getJointVelocity().size()); + data.vectors["joints_state::velocities::retargeting"].assign(m_retargetingClient->jointVelocities().data(), m_retargetingClient->jointVelocities().data() + m_retargetingClient->jointVelocities().size()); m_loggerPort.write(); @@ -1524,9 +1603,18 @@ bool WalkingModule::startWalking() return false; } + if (m_useBLFIK) + { + if (!m_BLFIKSolver->setRegularizationJointSetPoint(m_robotControlHelper->getJointPosition())) + { + yError() << "[WalkingModule::startWalking] Unable to set regularization joint value."; + return false; + } + } + // before running the controller the retargeting client goes in approaching phase this // guarantees a smooth transition - m_retargetingClient->setPhase(RetargetingClient::Phase::approacing); + m_retargetingClient->setPhase(RetargetingClient::Phase::Approaching); m_robotState = WalkingFSM::Walking; return true; diff --git a/src/WholeBodyControllers/CMakeLists.txt b/src/WholeBodyControllers/CMakeLists.txt index ccf63fd4..27406f6c 100644 --- a/src/WholeBodyControllers/CMakeLists.txt +++ b/src/WholeBodyControllers/CMakeLists.txt @@ -4,6 +4,10 @@ add_walking_controllers_library( NAME WholeBodyControllers - SOURCES src/InverseKinematics.cpp src/QPInverseKinematics.cpp src/QPInverseKinematics_osqp.cpp src/QPInverseKinematics_qpOASES.cpp - PUBLIC_HEADERS include/WalkingControllers/WholeBodyControllers/InverseKinematics.h include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h include/WalkingControllers/WholeBodyControllers/QPInverseKinematics_osqp.h include/WalkingControllers/WholeBodyControllers/QPInverseKinematics_qpOASES.h - PUBLIC_LINK_LIBRARIES WalkingControllers::YarpUtilities WalkingControllers::iDynTreeUtilities WalkingControllers::KinDynWrapper OsqpEigen::OsqpEigen ${qpOASES_LIBRARIES} Eigen3::Eigen ctrlLib) + SOURCES src/InverseKinematics.cpp src/QPInverseKinematics.cpp src/QPInverseKinematics_osqp.cpp src/QPInverseKinematics_qpOASES.cpp src/BLFIK.cpp + PUBLIC_HEADERS include/WalkingControllers/WholeBodyControllers/InverseKinematics.h + include/WalkingControllers/WholeBodyControllers/QPInverseKinematics.h + include/WalkingControllers/WholeBodyControllers/QPInverseKinematics_osqp.h + include/WalkingControllers/WholeBodyControllers/QPInverseKinematics_qpOASES.h + include/WalkingControllers/WholeBodyControllers/BLFIK.h + PUBLIC_LINK_LIBRARIES BipedalLocomotion::IK BipedalLocomotion::ContinuousDynamicalSystem WalkingControllers::YarpUtilities WalkingControllers::iDynTreeUtilities WalkingControllers::KinDynWrapper OsqpEigen::OsqpEigen ${qpOASES_LIBRARIES} Eigen3::Eigen ctrlLib) diff --git a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h new file mode 100644 index 00000000..1d19809b --- /dev/null +++ b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h @@ -0,0 +1,80 @@ +/** + * @file BLFIK.h + * @authors Giulio Romualdi + * @copyright 2022 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the LGPLv2.1 or later, see LGPL.TXT + */ + +#ifndef WALKING_CONTROLLERS_WHOLE_BODY_CONTROLLERS_BLF_IK +#define WALKING_CONTROLLERS_WHOLE_BODY_CONTROLLERS_BLF_IK + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace WalkingControllers +{ + +class BLFIK +{ +public: + bool initialize(std::weak_ptr handler, + std::shared_ptr kinDyn); + + bool setPhase(const std::string& phase); + + bool solve(); + + bool setLeftFootSetPoint(const iDynTree::Transform& desiredTransform, + const iDynTree::Twist& desiredVelocity); + + bool setRightFootSetPoint(const iDynTree::Transform& desiredTransform, + const iDynTree::Twist& desiredVelocity); + + bool setRetargetingJointSetPoint(const iDynTree::VectorDynSize& jointPositions, + const iDynTree::VectorDynSize& jointVelocities); + bool setRegularizationJointSetPoint(const iDynTree::VectorDynSize& jointPosition); + bool setCoMSetPoint(const iDynTree::Position& position, const iDynTree::Vector3& velocity); + bool setRootSetPoint(const iDynTree::Position& position, const iDynTree::Vector3& velocity); + bool setTorsoSetPoint(const iDynTree::Rotation& rotation); + const iDynTree::VectorDynSize& getDesiredJointVelocity() const; + +private: + std::shared_ptr + m_torsoWeight; + std::shared_ptr + m_jointRegularizationWeight; + std::shared_ptr + m_jointRetargetingWeight; + + BipedalLocomotion::IK::QPInverseKinematics m_qpIK; + BipedalLocomotion::System::VariablesHandler m_variableHandler; + + std::shared_ptr m_comTask; + std::shared_ptr m_torsoTask; + std::shared_ptr m_leftFootTask; + std::shared_ptr m_rightFootTask; + std::shared_ptr m_rootTask; + std::shared_ptr m_jointRetargetingTask; + std::shared_ptr m_jointRegularizationTask; + + iDynTree::VectorDynSize m_jointVelocity; + bool m_usejointRetargeting{false}; + bool m_useRootLinkForHeight{false}; + bool m_useFeedforwardTermForJointRetargeting{false}; +}; + +} // namespace WalkingControllers + +#endif // WALKING_CONTROLLERS_WHOLE_BODY_CONTROLLERS_BLF_IK diff --git a/src/WholeBodyControllers/src/BLFIK.cpp b/src/WholeBodyControllers/src/BLFIK.cpp new file mode 100644 index 00000000..93dfcaf6 --- /dev/null +++ b/src/WholeBodyControllers/src/BLFIK.cpp @@ -0,0 +1,225 @@ +/** + * @file BLFIK.h + * @authors Giulio Romualdi + * @copyright 2022 Istituto Italiano di Tecnologia (IIT). This software may be modified and + * distributed under the terms of the LGPLv2.1 or later, see LGPL.TXT + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include + +#include + +using namespace WalkingControllers; + +bool BLFIK::initialize( + std::weak_ptr handler, + std::shared_ptr kinDyn) +{ + constexpr auto prefix = "[BLFIK::initialize]"; + constexpr std::size_t highPriority = 0; + constexpr std::size_t lowPriority = 1; + + auto ptr = handler.lock(); + if (ptr == nullptr) + { + BipedalLocomotion::log()->error("{} Invalid parameter handler", prefix); + return false; + } + + m_usejointRetargeting = false; + ptr->getParameter("use_joint_retargeting", m_usejointRetargeting); + + ptr->getParameter("use_feedforward_term_for_joint_retargeting", + m_useFeedforwardTermForJointRetargeting); + + m_useRootLinkForHeight = false; + ptr->getParameter("use_root_link_for_height", m_useRootLinkForHeight); + + // weight providers + bool ok = m_qpIK.initialize(ptr->getGroup("IK")); + auto group = ptr->getGroup("IK").lock(); + std::string variable; + group->getParameter("robot_velocity_variable_name", variable); + m_variableHandler.addVariable(variable, kinDyn->getNrOfDegreesOfFreedom() + 6); + + m_torsoWeight + = std::make_shared(); + ok = ok && m_torsoWeight->initialize(ptr->getGroup("TORSO_TASK")); + + if (m_usejointRetargeting) + { + m_jointRetargetingWeight = std::make_shared< + BipedalLocomotion::ContinuousDynamicalSystem::MultiStateWeightProvider>(); + ok = ok && m_jointRetargetingWeight->initialize(ptr->getGroup("JOINT_RETARGETING_TASK")); + } + + m_jointRegularizationWeight + = std::make_shared(); + ok = ok && m_jointRegularizationWeight->initialize(ptr->getGroup("JOINT_REGULARIZATION_TASK")); + + // CoM Task + m_comTask = std::make_shared(); + ok = ok && m_comTask->setKinDyn(kinDyn); + ok = ok && m_comTask->initialize(ptr->getGroup("COM_TASK")); + ok = ok && m_qpIK.addTask(m_comTask, "com_task", highPriority); + + m_rightFootTask = std::make_shared(); + ok = ok && m_rightFootTask->setKinDyn(kinDyn); + ok = ok && m_rightFootTask->initialize(ptr->getGroup("RIGHT_FOOT_TASK")); + ok = ok && m_qpIK.addTask(m_rightFootTask, "right_foot_task", highPriority); + + m_leftFootTask = std::make_shared(); + ok = ok && m_leftFootTask->setKinDyn(kinDyn); + ok = ok && m_leftFootTask->initialize(ptr->getGroup("LEFT_FOOT_TASK")); + ok = ok && m_qpIK.addTask(m_leftFootTask, "left_foot_task", highPriority); + + m_torsoTask = std::make_shared(); + ok = ok && m_torsoTask->setKinDyn(kinDyn); + ok = ok && m_torsoTask->initialize(ptr->getGroup("TORSO_TASK")); + ok = ok && m_qpIK.addTask(m_torsoTask, "torso_task", lowPriority, m_torsoWeight); + + m_jointRegularizationTask = std::make_shared(); + ok = ok && m_jointRegularizationTask->setKinDyn(kinDyn); + ok = ok && m_jointRegularizationTask->initialize(ptr->getGroup("JOINT_REGULARIZATION_TASK")); + ok = ok + && m_qpIK.addTask(m_jointRegularizationTask, + "joint_regularization_task", + lowPriority, + m_jointRegularizationWeight); + + if (m_usejointRetargeting) + { + m_jointRetargetingTask = std::make_shared(); + ok = ok && m_jointRetargetingTask->setKinDyn(kinDyn); + ok = ok && m_jointRetargetingTask->initialize(ptr->getGroup("JOINT_RETARGETING_TASK")); + ok = ok + && m_qpIK.addTask(m_jointRetargetingTask, + "joint_retargeting_task", + lowPriority, + m_jointRetargetingWeight); + } + + if (m_useRootLinkForHeight) + { + m_rootTask = std::make_shared(); + ok = ok && m_rootTask->setKinDyn(kinDyn); + ok = ok && m_rootTask->initialize(ptr->getGroup("ROOT_TASK")); + ok = ok && m_qpIK.addTask(m_rootTask, "root_task", highPriority); + } + + ok = ok && m_qpIK.finalize(m_variableHandler); + + BipedalLocomotion::log()->info("[BLFIK::initialize] {}", m_qpIK.toString()); + + m_jointVelocity.resize(kinDyn->getNrOfDegreesOfFreedom()); + + return ok; +} + +bool BLFIK::solve() +{ + bool ok = m_torsoWeight->advance(); + ok = ok && m_jointRegularizationWeight->advance(); + if (m_usejointRetargeting) + { + ok = ok && m_jointRetargetingWeight->advance(); + } + + ok = ok && m_qpIK.advance(); + ok = ok && m_qpIK.isOutputValid(); + + if (ok) + { + iDynTree::toEigen(m_jointVelocity) = m_qpIK.getOutput().jointVelocity; + } + + return ok; +} + +bool BLFIK::setPhase(const std::string& phase) +{ + bool ok = m_torsoWeight->setState(phase); + ok = ok && m_jointRegularizationWeight->setState(phase); + + if (m_usejointRetargeting) + ok = ok && m_jointRetargetingWeight->setState(phase); + + return ok; +} + +bool BLFIK::setLeftFootSetPoint(const iDynTree::Transform& desiredTransform, + const iDynTree::Twist& desiredVelocity) +{ + return m_leftFootTask + ->setSetPoint(BipedalLocomotion::Conversions::toManifPose(desiredTransform), + BipedalLocomotion::Conversions::toManifTwist(desiredVelocity)); +} + +bool BLFIK::setRightFootSetPoint(const iDynTree::Transform& desiredTransform, + const iDynTree::Twist& desiredVelocity) +{ + return m_rightFootTask + ->setSetPoint(BipedalLocomotion::Conversions::toManifPose(desiredTransform), + BipedalLocomotion::Conversions::toManifTwist(desiredVelocity)); +} + +bool BLFIK::setRetargetingJointSetPoint(const iDynTree::VectorDynSize& jointPositions, + const iDynTree::VectorDynSize& jointVelocities) +{ + if (m_usejointRetargeting) + { + if (m_useFeedforwardTermForJointRetargeting) + { + return m_jointRetargetingTask->setSetPoint(iDynTree::toEigen(jointPositions), + iDynTree::toEigen(jointVelocities)); + } else + { + return m_jointRetargetingTask->setSetPoint(iDynTree::toEigen(jointPositions)); + } + } + return true; +} + +bool BLFIK::setRegularizationJointSetPoint(const iDynTree::VectorDynSize& jointPosition) +{ + return m_jointRegularizationTask->setSetPoint(iDynTree::toEigen(jointPosition)); +} + +bool BLFIK::setCoMSetPoint(const iDynTree::Position& position, const iDynTree::Vector3& velocity) +{ + return m_comTask->setSetPoint(iDynTree::toEigen(position), iDynTree::toEigen(velocity)); +} + +bool BLFIK::setRootSetPoint(const iDynTree::Position& position, const iDynTree::Vector3& velocity) +{ + if (m_useRootLinkForHeight) + { + return m_rootTask->setSetPoint(iDynTree::toEigen(position), iDynTree::toEigen(velocity)); + } + return true; +} + +bool BLFIK::setTorsoSetPoint(const iDynTree::Rotation& rotation) +{ + return m_torsoTask->setSetPoint(BipedalLocomotion::Conversions::toManifRot(rotation)); +} + +const iDynTree::VectorDynSize& BLFIK::getDesiredJointVelocity() const +{ + return m_jointVelocity; +} diff --git a/src/YarpUtilities/CMakeLists.txt b/src/YarpUtilities/CMakeLists.txt index 61de8c24..bf9996fd 100644 --- a/src/YarpUtilities/CMakeLists.txt +++ b/src/YarpUtilities/CMakeLists.txt @@ -7,3 +7,8 @@ add_walking_controllers_library( SOURCES src/Helper.cpp PUBLIC_HEADERS include/WalkingControllers/YarpUtilities/Helper.h include/WalkingControllers/YarpUtilities/Helper.tpp PUBLIC_LINK_LIBRARIES ${YARP_LIBRARIES}) + +add_walking_controllers_yarp_thrift( + NAME HumanState + THRIFT thrifts/WalkingControllers/YarpUtilities/HumanState.thrift + INSTALLATION_FOLDER YarpUtilities) diff --git a/src/YarpUtilities/thrifts/WalkingControllers/YarpUtilities/HumanState.thrift b/src/YarpUtilities/thrifts/WalkingControllers/YarpUtilities/HumanState.thrift new file mode 100644 index 00000000..543d07e3 --- /dev/null +++ b/src/YarpUtilities/thrifts/WalkingControllers/YarpUtilities/HumanState.thrift @@ -0,0 +1,34 @@ +namespace yarp WalkingControllers.YarpUtilities + +/** + * Representation of a 3D vector + */ +struct Vector3 { + 1: double x; + 2: double y; + 3: double z; +} + +/** + * Representation of a Quaternion + */ +struct Quaternion { + 1: double w; + 2: Vector3 imaginary; +} + +/** + * Representation of the IHumanState interface + */ +struct HumanState { + 1: list jointNames; + 2: list positions; + 3: list velocities; + + 4: string baseName; + 5: Vector3 baseOriginWRTGlobal; + 6: Quaternion baseOrientationWRTGlobal; + 7: list baseVelocityWRTGlobal; + 8: Vector3 CoMPositionWRTGlobal; + 9: Vector3 CoMVelocityWRTGlobal; +}