diff --git a/cmake/WalkingControllersDependencies.cmake b/cmake/WalkingControllersDependencies.cmake index 6d07217e..d29d6cf2 100644 --- a/cmake/WalkingControllersDependencies.cmake +++ b/cmake/WalkingControllersDependencies.cmake @@ -10,7 +10,8 @@ find_package(Eigen3 3.2.92 REQUIRED) find_package(UnicyclePlanner 0.5.2 REQUIRED) find_package(OsqpEigen 0.4.0 REQUIRED) find_package(qpOASES REQUIRED) -find_package(BipedalLocomotionFramework 0.9.0 +find_package(BipedalLocomotionFramework 0.10.0 COMPONENTS VectorsCollection IK ParametersHandlerYarpImplementation ContinuousDynamicalSystem ManifConversions - ParametersHandlerYarpImplementation REQUIRED) + ParametersHandlerYarpImplementation + RobotInterfaceYarpImplementation REQUIRED) diff --git a/src/WalkingModule/CMakeLists.txt b/src/WalkingModule/CMakeLists.txt index a01fbf76..07dace0d 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 BipedalLocomotion::ParametersHandlerYarpImplementation 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 BipedalLocomotion::ParametersHandlerYarpImplementation BipedalLocomotion::RobotInterfaceYarpImplementation ctrlLib SUBDIRECTORIES app) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/sensorBridge.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/sensorBridge.ini new file mode 100644 index 00000000..d1fbdeb9 --- /dev/null +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking/common/sensorBridge.ini @@ -0,0 +1,29 @@ +check_for_nan false +stream_joint_states false +stream_cartesian_wrenches false +stream_inertials true + +[InertialSensors] +orientation_sensors_list (l_foot_front_ft_eul, l_foot_rear_ft_eul, r_foot_front_ft_eul, r_foot_rear_ft_eul) + +[LEFT_FOOT] +description left_foot +remote_port_name /icub/left_foot/imu +local_prefix walking-coordinator +local_port_name_postfix /left_foot/imu +controlled_frame_name l_sole +imu_names (l_foot_front_ft_eul, l_foot_rear_ft_eul) +frame_names (l_foot_front_ft_sensor, l_foot_rear_ft_sensor) + +[RIGHT_FOOT] +description right_foot +remote_port_name /icub/right_foot/imu +local_prefix walking-coordinator +local_port_name_postfix /right_foot/imu +controlled_frame_name r_sole +imu_names (r_foot_front_ft_eul, r_foot_rear_ft_eul) +frame_names (r_foot_front_ft_sensor, r_foot_rear_ft_sensor) + +[MULTIPLE_ANALOG_SENSORS_REMAPPER] +description remapper +orientation_sensors_names (l_foot_front_ft_eul, l_foot_rear_ft_eul, r_foot_front_ft_eul, r_foot_rear_ft_eul) 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 index 87c2081f..4b75c831 100644 --- 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 @@ -6,7 +6,7 @@ kp (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 +sampling_time 0.002 settling_time 0.5 [stance] 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 index 8258c0af..89f8654e 100644 --- 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 @@ -4,7 +4,7 @@ kp_angular 5.0 states ("stance", "walking") -sampling_time 0.01 +sampling_time 0.002 settling_time 0.5 [stance] 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 8eedaea4..40c9a142 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 @@ -39,7 +39,7 @@ goal_port_suffix /goal:i goal_port_scaling (0.5, 1.0, 0.5) # How much in advance the planner should be called. The time is in seconds -planner_advance_time_in_s 0.08 +planner_advance_time_in_s 0.02 # general parameters [GENERAL] @@ -56,6 +56,8 @@ use_com_retargeting 0 # Specify the frame to use to control the robot height. Currently, we support only the following options: com, root_link height_reference_frame root_link + + # include robot control parameters [include ROBOT_CONTROL "./dcm_walking/iFeel_joint_retargeting/robotControl.ini"] @@ -97,3 +99,6 @@ height_reference_frame root_link # retargeting [include RETARGETING "./dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini"] + +# include sensor bridge parameters +[include SENSOR_BRIDGE "./dcm_walking/common/sensorBridge.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 e13120db..5db8c18b 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini @@ -39,7 +39,7 @@ goal_port_suffix /goal:i goal_port_scaling (0.5, 1.0, 0.5) # How much in advance the planner should be called. The time is in seconds -planner_advance_time_in_s 0.08 +planner_advance_time_in_s 0.02 # general parameters [GENERAL] @@ -47,9 +47,11 @@ name walking-coordinator # height of the com com_height 0.565 # sampling time -sampling_time 0.01 +sampling_time 0.002 # Specify the frame to use to control the robot height. Currently, we support only the following options: com, root_link height_reference_frame root_link +use_left_foot_imu true +use_right_foot_imu true # include robot control parameters [include ROBOT_CONTROL "./dcm_walking/joypad_control/robotControl.ini"] @@ -89,3 +91,6 @@ height_reference_frame root_link # include lower PID parameters [include PID "./dcm_walking/common/pidParams.ini"] + +# include sensor bridge parameters +[include SENSOR_BRIDGE "./dcm_walking/common/sensorBridge.ini"] diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 7f472d5e..f0a977a7 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -10,45 +10,46 @@ #define WALKING_MODULE_HPP // std -#include +#include +#include #include #include // YARP +#include #include -#include - #include +#include - +#include +#include #include // iDynTree #include #include +#include +#include +#include // WalkingControllers library +#include +#include #include #include -#include -#include -#include - #include #include #include - +#include +#include +#include +#include +#include #include #include #include #include -#include - -#include - -#include - // iCub-ctrl #include @@ -65,6 +66,8 @@ namespace WalkingControllers enum class WalkingFSM {Idle, Configured, Preparing, Prepared, Walking, Paused, Stopped}; WalkingFSM m_robotState{WalkingFSM::Idle}; /**< State of the WalkingFSM. */ + std::vector m_polyDrivers; + double m_dT; /**< RFModule period. */ double m_time; /**< Current time. */ std::string m_robot; /**< Robot name. */ @@ -73,6 +76,7 @@ namespace WalkingControllers 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_useFeetImu{false}; /**< True if you want to use the imu on the feet. */ bool m_dumpData; /**< True if data are saved. */ bool m_firstRun; /**< True if it is the first run. */ bool m_skipDCMController; /**< True if the desired ZMP should be used instead of the DCM controller. */ @@ -101,6 +105,8 @@ namespace WalkingControllers std::unique_ptr m_retargetingClient; /**< Pointer to the stable DCM dynamics. */ std::unique_ptr m_profiler; /**< Time profiler. */ + BipedalLocomotion::RobotInterface::YarpSensorBridge m_sensorBridge; /**< Helper to get the data from the robot */ + double m_additionalRotationWeightDesired; /**< Desired additional rotational weight matrix. */ double m_desiredJointsWeight; /**< Desired joint weight matrix. */ yarp::sig::Vector m_desiredJointInRadYarp; /**< Desired joint position (regularization task). */ @@ -153,6 +159,23 @@ namespace WalkingControllers yarp::os::BufferedPort m_loggerPort; /**< Logger port. */ + struct IMUOrientationData + { + iDynTree::Rotation IMU_R_controlledFrame; + iDynTree::FrameIndex frameIndex; + iDynTree::Rotation I_R_I_IMU; + iDynTree::Rotation I_R_IMU; + iDynTree::Rotation I_R_controlledFrame; + }; + + struct linkIMU + { + std::unordered_map IMUs; + iDynTree::Rotation averageRotation; + }; + + std::unordered_map m_linksWithIMU; + /** * Get the robot model from the resource finder and set it. * @param rf is the reference to a resource finder object. @@ -264,6 +287,18 @@ namespace WalkingControllers */ void applyGoalScaling(yarp::sig::Vector &plannerInput); + + /** + * Configure the SensorBridge. + * @param rf is the reference to a resource finder object + * @return true in case of success and false otherwise. + */ + bool configureSensorBridge(const yarp::os::Bottle& rf); + + bool configureLinkWithIMU( + std::weak_ptr handler, + const std::string& linkName); + public: /** diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 085cbade..f48ab1c3 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -12,6 +12,7 @@ #include // YARP +#include #include #include #include @@ -23,6 +24,9 @@ #include #include #include +#include +#include +#include // blf #include @@ -134,6 +138,152 @@ bool WalkingModule::setRobotModel(const yarp::os::Searchable& rf) return true; } +bool WalkingModule::configureLinkWithIMU( + std::weak_ptr handler, + const std::string& linkName) +{ + auto linkPoly + = BipedalLocomotion::RobotInterface::constructMultipleAnalogSensorsClient(handler); + if (!linkPoly.isValid()) + { + yError() << "[WalkingModule::configureLinkWithIMU] Unable to initialize the left foot imu"; + return false; + } + + auto linkHandler = handler.lock(); + std::vector imuNames; + if (!linkHandler->getParameter("imu_names", imuNames)) + { + yError() << "[WalkingModule::configureLinkWithIMU] Unable to get the imu names"; + return false; + } + + std::vector frameNames; + if (!linkHandler->getParameter("frame_names", frameNames)) + { + yError() << "[WalkingModule::configureLinkWithIMU] Unable to get the frames names associated to each imu"; + return false; + } + + std::string controlledFrameName; + if (!linkHandler->getParameter("controlled_frame_name", controlledFrameName)) + { + yError() << "[WalkingModule::configureLinkWithIMU] Unable to get the name of the controlled frame"; + return false; + } + iDynTree::FrameIndex controlledFrameIndex + = m_FKSolver->getKinDyn()->model().getFrameIndex(controlledFrameName); + + if (controlledFrameIndex == iDynTree::FRAME_INVALID_INDEX) + { + yError() << "[WalkingModule::configureLinkWithIMU] Unable to find the frame named " + << controlledFrameName; + return false; + } + + const iDynTree::Rotation link_R_controlledFrame + = m_FKSolver->getKinDyn()->model().getFrameTransform(controlledFrameIndex).getRotation(); + + if (frameNames.size() != imuNames.size()) + { + yError() << "[WalkingModule::configureLinkWithIMU] Mismatch between the number of imu and " + "the name of the frames"; + return false; + } + + for (int i = 0; i < frameNames.size(); i++) + { + + m_linksWithIMU[linkName].IMUs[imuNames[i]].frameIndex + = m_FKSolver->getKinDyn()->model().getFrameIndex(frameNames[i]); + + if (m_linksWithIMU[linkName].IMUs[imuNames[i]].frameIndex == iDynTree::FRAME_INVALID_INDEX) + { + yError() << "[WalkingModule::configureLinkWithIMU] Unable to find the frame named " + << frameNames[i]; + return false; + } + + // check that the frame link between the imu frame and the controlled frame is the same + if (m_FKSolver->getKinDyn()->model().getFrameLink(controlledFrameIndex) + != m_FKSolver->getKinDyn()->model().getFrameLink( + m_linksWithIMU[linkName].IMUs[imuNames[i]].frameIndex)) + { + yError() << "[WalkingModule::configureLinkWithIMU] The controlled frame and the IMU " + "frame should belong to the smae link"; + return false; + } + m_linksWithIMU[linkName].IMUs[imuNames[i]].IMU_R_controlledFrame + = m_FKSolver->getKinDyn() + ->model() + .getFrameTransform(m_linksWithIMU[linkName].IMUs[imuNames[i]].frameIndex) + .getRotation() + .inverse() + * link_R_controlledFrame; + } + + m_polyDrivers.push_back(linkPoly); + + return true; +} + +bool WalkingModule::configureSensorBridge(const yarp::os::Bottle& rf) +{ + auto handler = std::make_shared(); + handler->set(rf); + + if(!this->configureLinkWithIMU(handler->getGroup("RIGHT_FOOT"), "right_foot")) + { + yError() << "[WalkingModule::configureSensorBridge] Unable to initialize the right foot imu"; + return false; + } + + if(!this->configureLinkWithIMU(handler->getGroup("LEFT_FOOT"), "left_foot")) + { + yError() << "[WalkingModule::configureSensorBridge] Unable to initialize the left foot imu"; + return false; + } + + auto polyRemapper = BipedalLocomotion::RobotInterface::constructMultipleAnalogSensorsRemapper( + handler->getGroup("MULTIPLE_ANALOG_SENSORS_REMAPPER"), m_polyDrivers); + + std::cerr << polyRemapper.poly << std::endl; + std::cerr << polyRemapper.key << std::endl; + + if(!polyRemapper.isValid()) + { + yError() << "[WalkingModule::configureSensorBridge] Unable to initialize the remapper"; + return false; + } + m_polyDrivers.push_back(polyRemapper); + + + yarp::dev::PolyDriverList list; + list.push(polyRemapper.poly.get(), polyRemapper.key.c_str()); + // for (auto& polyDriver : m_polyDrivers) + // { + // list.push(polyDriver.poly.get(), polyDriver.key.c_str()); + // } + + if (!m_sensorBridge.initialize(handler)) + { + yError() << "[WalkingModule::configureSensorBridge] Unable to initialize the sensor bridge"; + return false; + } + + + using namespace std::chrono_literals; + std::this_thread::sleep_for(2000ms); + + if (!m_sensorBridge.setDriversList(list)) + { + yError() << "[WalkingModule::configureSensorBridge] Unable to set the driver list"; + return false; + } + + return true; +} + bool WalkingModule::configure(yarp::os::ResourceFinder& rf) { // module name (used as prefix for opened ports) @@ -391,6 +541,18 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) return false; } + bool useLeftFootImu = generalOptions.check("use_left_foot_imu", yarp::os::Value(false)).asBool(); + bool useRightFootImu = generalOptions.check("use_right_foot_imu", yarp::os::Value(false)).asBool(); + m_useFeetImu = true || useLeftFootImu || useRightFootImu; + if(m_useFeetImu) + { + yarp::os::Bottle sensorBridgeOptions = rf.findGroup("SENSOR_BRIDGE"); + if(!configureSensorBridge(sensorBridgeOptions)) + { + yError() << "[WalkingModule::configure] Unable to use the feet imu"; + return false; + } + } // initialize the logger if(m_dumpData) { @@ -596,6 +758,16 @@ bool WalkingModule::solveBLFIK(const iDynTree::Position& desiredCoMPosition, && m_BLFIKSolver->setRetargetingJointSetPoint(m_retargetingClient->jointPositions(), m_retargetingClient->jointVelocities()); + if (m_useFeetImu) + { + ok = ok + && m_BLFIKSolver->setLeftFootMeasuredOrientation( + m_linksWithIMU["left_foot"].averageRotation); + ok = ok + && m_BLFIKSolver->setRightFootMeasuredOrientation( + m_linksWithIMU["right_foot"].averageRotation); + } + if (m_useRootLinkForHeight) { ok = ok && m_BLFIKSolver->setRootSetPoint(desiredCoMPosition, desiredCoMVelocity); @@ -725,6 +897,39 @@ bool WalkingModule::updateModule() m_profiler->setInitTime("Total"); + // get the imu + if (m_useFeetImu) + { + m_sensorBridge.advance(); + iDynTree::Vector3 rpyTemp; + + for (auto& [linkName, imu] : m_linksWithIMU) + { + std::vector tempRots; + for (auto& [imuName, orientationData] : imu.IMUs) + { + m_sensorBridge.getOrientationSensorMeasurement(imuName, + iDynTree::toEigen(rpyTemp)); + orientationData.I_R_IMU + = orientationData.I_R_I_IMU + * iDynTree::Rotation::RPY(rpyTemp(0), rpyTemp(1), rpyTemp(2)); + + // remove the yaw + iDynTree::Vector3 fkRPY = m_FKSolver->getKinDyn() + ->getWorldTransform(orientationData.frameIndex) + .getRotation() + .asRPY(); + + iDynTree::Vector3 I_R_IMU_rpy = orientationData.I_R_IMU.asRPY(); + orientationData.I_R_controlledFrame + = iDynTree::Rotation::RPY(I_R_IMU_rpy(0), I_R_IMU_rpy(1), fkRPY(2)) + * orientationData.IMU_R_controlledFrame; + tempRots.push_back(orientationData.I_R_controlledFrame); + } + + iDynTree::geodesicL2MeanRotation(tempRots, imu.averageRotation); + } + } // check desired planner input yarp::sig::Vector* desiredUnicyclePosition = nullptr; desiredUnicyclePosition = m_desiredUnyciclePositionPort.read(false); @@ -1086,6 +1291,13 @@ bool WalkingModule::updateModule() // Left foot orientation iDynTree::Vector3 leftFootOrientationMeasured = leftFoot.getRotation().asRPY(); data.vectors["left_foot::orientation::measured"].assign(leftFootOrientationMeasured.begin(), leftFootOrientationMeasured.begin() + leftFootOrientationMeasured.size()); + + if(m_useFeetImu) + { + iDynTree::Vector3 leftFootOrientationMeasuredIMU = m_linksWithIMU["left_foot"].averageRotation.asRPY(); + data.vectors["left_foot::orientation::measured_imu"].assign(leftFootOrientationMeasuredIMU.begin(), leftFootOrientationMeasuredIMU.begin() + leftFootOrientationMeasuredIMU.size()); + } + iDynTree::Vector3 leftFootOrientationDesired = m_leftTrajectory.front().getRotation().asRPY(); data.vectors["left_foot::orientation::desired"].assign(leftFootOrientationDesired.begin(), leftFootOrientationDesired.begin() + leftFootOrientationDesired.size()); @@ -1106,6 +1318,12 @@ bool WalkingModule::updateModule() // Right foot orientation iDynTree::Vector3 rightFootOrientationMeasured = rightFoot.getRotation().asRPY(); data.vectors["right_foot::orientation::measured"].assign(rightFootOrientationMeasured.begin(), rightFootOrientationMeasured.begin() + rightFootOrientationMeasured.size()); + if(m_useFeetImu) + { + iDynTree::Vector3 rightFootOrientationMeasuredIMU = m_linksWithIMU["right_foot"].averageRotation.asRPY(); + data.vectors["right_foot::orientation::measured_imu"].assign(rightFootOrientationMeasuredIMU.begin(), rightFootOrientationMeasuredIMU.begin() + rightFootOrientationMeasuredIMU.size()); + } + iDynTree::Vector3 rightFootOrientationDesired = m_rightTrajectory.front().getRotation().asRPY(); data.vectors["right_foot::orientation::desired"].assign(rightFootOrientationDesired.begin(), rightFootOrientationDesired.begin() + rightFootOrientationDesired.size()); @@ -1623,6 +1841,24 @@ bool WalkingModule::startWalking() } } + // reset the world frame for the imus + if (m_useFeetImu) + { + iDynTree::Vector3 rpyTemp; + m_sensorBridge.advance(); + for (auto& [linkName, imu] : m_linksWithIMU) + { + for (auto& [imuName, orientationData] : imu.IMUs) + { + m_sensorBridge.getOrientationSensorMeasurement(imuName, iDynTree::toEigen(rpyTemp)); + orientationData.I_R_I_IMU + = m_FKSolver->getKinDyn() + ->getWorldTransform(orientationData.frameIndex) + .getRotation() + * iDynTree::Rotation::RPY(rpyTemp(0), rpyTemp(1), rpyTemp(2)).inverse(); + } + } + } // before running the controller the retargeting client goes in approaching phase this // guarantees a smooth transition m_retargetingClient->setPhase(RetargetingClient::Phase::Approaching); diff --git a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h index 1d19809b..112984c2 100644 --- a/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h +++ b/src/WholeBodyControllers/include/WalkingControllers/WholeBodyControllers/BLFIK.h @@ -48,6 +48,11 @@ class BLFIK 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); + + bool setLeftFootMeasuredOrientation(const iDynTree::Rotation& I_R_LF); + bool setRightFootMeasuredOrientation(const iDynTree::Rotation& I_R_RF); + + const iDynTree::VectorDynSize& getDesiredJointVelocity() const; private: @@ -73,6 +78,8 @@ class BLFIK bool m_usejointRetargeting{false}; bool m_useRootLinkForHeight{false}; bool m_useFeedforwardTermForJointRetargeting{false}; + bool m_useExternalFeedbackForLeftFootOrientation{false}; + bool m_useExternalFeedbackForRightFootOrientation{false}; }; } // namespace WalkingControllers diff --git a/src/WholeBodyControllers/src/BLFIK.cpp b/src/WholeBodyControllers/src/BLFIK.cpp index 93dfcaf6..27970efa 100644 --- a/src/WholeBodyControllers/src/BLFIK.cpp +++ b/src/WholeBodyControllers/src/BLFIK.cpp @@ -50,6 +50,11 @@ bool BLFIK::initialize( m_useRootLinkForHeight = false; ptr->getParameter("use_root_link_for_height", m_useRootLinkForHeight); + m_useExternalFeedbackForLeftFootOrientation = false; + m_useExternalFeedbackForRightFootOrientation = false; + ptr->getParameter("use_left_foot_imu", m_useExternalFeedbackForLeftFootOrientation); + ptr->getParameter("use_right_foot_imu", m_useExternalFeedbackForRightFootOrientation); + // weight providers bool ok = m_qpIK.initialize(ptr->getGroup("IK")); auto group = ptr->getGroup("IK").lock(); @@ -78,16 +83,36 @@ bool BLFIK::initialize( ok = ok && m_comTask->initialize(ptr->getGroup("COM_TASK")); ok = ok && m_qpIK.addTask(m_comTask, "com_task", highPriority); + // right foot task m_rightFootTask = std::make_shared(); ok = ok && m_rightFootTask->setKinDyn(kinDyn); - ok = ok && m_rightFootTask->initialize(ptr->getGroup("RIGHT_FOOT_TASK")); + auto ptrRight = ptr->getGroup("RIGHT_FOOT_TASK").lock(); + if (ptrRight == nullptr) + { + BipedalLocomotion::log()->error("{} Unable to find the 'RIGHT_FOOT_TASK' group.", prefix); + return false; + } + ptrRight->setParameter("use_orientation_exogenous_feedback", m_useExternalFeedbackForRightFootOrientation); + ok = ok && m_rightFootTask->initialize(ptrRight); ok = ok && m_qpIK.addTask(m_rightFootTask, "right_foot_task", highPriority); + + // left foot task m_leftFootTask = std::make_shared(); ok = ok && m_leftFootTask->setKinDyn(kinDyn); - ok = ok && m_leftFootTask->initialize(ptr->getGroup("LEFT_FOOT_TASK")); + + auto ptrLeft = ptr->getGroup("LEFT_FOOT_TASK").lock(); + if (ptrLeft == nullptr) + { + BipedalLocomotion::log()->error("{} Unable to find the 'LEFT_FOOT_TASK' group.", prefix); + return false; + } + ptrLeft->setParameter("use_orientation_exogenous_feedback", + m_useExternalFeedbackForLeftFootOrientation); + ok = ok && m_leftFootTask->initialize(ptrLeft); ok = ok && m_qpIK.addTask(m_leftFootTask, "left_foot_task", highPriority); + // torso m_torsoTask = std::make_shared(); ok = ok && m_torsoTask->setKinDyn(kinDyn); ok = ok && m_torsoTask->initialize(ptr->getGroup("TORSO_TASK")); @@ -219,6 +244,24 @@ bool BLFIK::setTorsoSetPoint(const iDynTree::Rotation& rotation) return m_torsoTask->setSetPoint(BipedalLocomotion::Conversions::toManifRot(rotation)); } +bool BLFIK::setLeftFootMeasuredOrientation(const iDynTree::Rotation& I_R_LF) +{ + if (m_useExternalFeedbackForLeftFootOrientation) + { + return m_leftFootTask->setFeedback(BipedalLocomotion::Conversions::toManifRot(I_R_LF)); + } + return true; +} + +bool BLFIK::setRightFootMeasuredOrientation(const iDynTree::Rotation& I_R_RF) +{ + if (m_useExternalFeedbackForRightFootOrientation) + { + return m_rightFootTask->setFeedback(BipedalLocomotion::Conversions::toManifRot(I_R_RF)); + } + return true; +} + const iDynTree::VectorDynSize& BLFIK::getDesiredJointVelocity() const { return m_jointVelocity;