diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 9f079c7a..71c5d5d1 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -14,3 +14,4 @@ add_subdirectory(KinDynWrapper) add_subdirectory(RetargetingHelper) add_subdirectory(WalkingModule) add_subdirectory(JoypadModule) +add_subdirectory(NavigationHelper) diff --git a/src/NavigationHelper/CMakeLists.txt b/src/NavigationHelper/CMakeLists.txt new file mode 100644 index 00000000..1171ea8e --- /dev/null +++ b/src/NavigationHelper/CMakeLists.txt @@ -0,0 +1,10 @@ +# Copyright (C) 2019 Fondazione Istituto Italiano di Tecnologia (IIT) +# All Rights Reserved. +# Authors: Giulio Romualdi + +add_walking_controllers_library( + NAME NavigationHelper + SOURCES src/NavigationHelper.cpp + PUBLIC_HEADERS include/WalkingControllers/NavigationHelper/NavigationHelper.h + PUBLIC_LINK_LIBRARIES WalkingControllers::YarpUtilities WalkingControllers::iDynTreeUtilities WalkingControllers::KinDynWrapper WalkingControllers::TrajectoryPlanner WalkingControllers::StdUtilities ctrlLib + PRIVATE_LINK_LIBRARIES Eigen3::Eigen) diff --git a/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h b/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h new file mode 100644 index 00000000..07bc3032 --- /dev/null +++ b/src/NavigationHelper/include/WalkingControllers/NavigationHelper/NavigationHelper.h @@ -0,0 +1,68 @@ +/** + * @file NavigationHelper.h + * @authors Simone Micheletti + * @copyright 2023 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2023 + */ + +#ifndef NAVIGATION_HELPER__HPP +#define NAVIGATION_HELPER__HPP + +#include +#include +#include +#include +#include + +//#include +#include +#include +#include + +namespace WalkingControllers +{ + class NavigationHelper + { + private: + yarp::os::BufferedPort m_unicyclePort; /**< Port that streams odometry info of the virtual unicycle. */ + yarp::os::BufferedPort m_feetPort; /**< Feet port vector of feet positions (left, right). */ + yarp::os::BufferedPort m_plannedCoMPositionPort; /**< Desired CoM position port for naviagation purposes. */ + std::atomic m_runThreads{false}; + std::deque* m_leftInContact; + std::deque* m_rightInContact; + int m_navigationTriggerLoopRate; /**< Loop rate for the thread computing the navigation trigger*/ + bool m_wasInDoubleSupport; + bool m_publishInfo; + std::deque m_desiredCoM_Trajectory; /**< Deque containing the desired CoM trajectory projected on the ground in pose x, y, theta. */ + + std::thread m_virtualUnicyclePubliserThread; /**< Thread for publishing the state of the unicycle used in the TrajectoryGenerator. */ + + const std::string m_portPrefix = "/navigation_helper"; + + public: + NavigationHelper(); + ~NavigationHelper(); + + bool closeThreads(); + bool closeHelper(); + bool init(const yarp::os::Searchable& config, std::deque &leftInContact, std::deque &rightInContact, + std::unique_ptr &FKSolver, + std::unique_ptr &stableDCMModel, + std::unique_ptr &trajectoryGenerator + ); + void computeVirtualUnicycleThread(std::unique_ptr &FKSolver, + std::unique_ptr &stableDCMModel, + std::unique_ptr &trajectoryGenerator + ); + + bool publishPlannedFootsteps(std::unique_ptr &trajectoryGenerator); + bool publishCoM(bool newTrajectoryMerged, + std::deque &m_DCMPositionDesired, + std::unique_ptr &m_stableDCMModel, + std::deque &m_leftTrajectory, + std::deque &m_rightTrajectory); + }; +} + +#endif diff --git a/src/NavigationHelper/src/NavigationHelper.cpp b/src/NavigationHelper/src/NavigationHelper.cpp new file mode 100644 index 00000000..d70f6351 --- /dev/null +++ b/src/NavigationHelper/src/NavigationHelper.cpp @@ -0,0 +1,360 @@ +/** + * @file NavigationHelper.cpp + * @authors Simone Micheletti + * @copyright 2023 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2023 + */ + +#include +#include +#include + +#include +#include + +#include +#include + + +using namespace WalkingControllers; + +NavigationHelper::NavigationHelper() +{ + m_wasInDoubleSupport = false; +} + +NavigationHelper::~NavigationHelper() +{ +} + +bool NavigationHelper::closeThreads() +{ + //Close parallel threads + if (m_runThreads) + { + m_runThreads = false; + yInfo() << "Closing m_virtualUnicyclePubliserThread"; + if(m_virtualUnicyclePubliserThread.joinable()) + { + m_virtualUnicyclePubliserThread.join(); + m_virtualUnicyclePubliserThread = std::thread(); + } + } + return true; +} + +bool NavigationHelper::closeHelper() +{ + try + { + //close threads first + if (m_runThreads) + { + closeThreads(); + } + //close ports + if(!m_unicyclePort.isClosed()) + m_unicyclePort.close(); + if(!m_feetPort.isClosed()) + m_feetPort.close(); + if(!m_plannedCoMPositionPort.isClosed()) + m_plannedCoMPositionPort.close(); + } + catch(const std::exception& e) + { + std::cerr << e.what() << std::endl; + return false; + } + return true; +} + +bool NavigationHelper::init(const yarp::os::Searchable& config, std::deque &leftInContact, std::deque &rightInContact, + std::unique_ptr &FKSolver, + std::unique_ptr &stableDCMModel, + std::unique_ptr &trajectoryGenerator) +{ + m_leftInContact = &leftInContact; + m_rightInContact = &rightInContact; + + m_navigationTriggerLoopRate = config.check("navigationTriggerLoopRate", yarp::os::Value(100)).asInt32(); + m_publishInfo = config.check("publishNavigationInfo", yarp::os::Value(false)).asBool(); + if (config.check("plannerMode", yarp::os::Value("manual")).asString() == "navigation") + { + //if in navigation mode we need this parameter to be true + m_publishInfo = true; + } + + // format check + if (m_navigationTriggerLoopRate<=0) + { + yError() << "[configure] navigationTriggerLoopRate must be strictly positive, instead is: " << m_navigationTriggerLoopRate; + return false; + } + if (!m_publishInfo) + { //exit the funnction if the infos are not needed + yInfo() << "[NavigationHelper::init] Configuring NavigationHelper without publishing infos on ports "; + return true; + } + + //ports for navigation integration + std::string unicyclePortPortName = m_portPrefix + "/virtual_unicycle_states:o"; + if (!m_unicyclePort.open(unicyclePortPortName)) + { + yError() << "[WalkingModule::configure] Could not open virtual_unicycle_states port"; + return false; + } + + std::string feetPositionsPortPortName = m_portPrefix + "/feet_positions:o"; + if (!m_feetPort.open(feetPositionsPortPortName)) + { + yError() << "[WalkingModule::configure] Could not open feet_positions port"; + return false; + } + + // open CoM planned trajectory port for navigation integration + std::string plannedCoMPositionPortName = m_portPrefix + "/planned_CoM/data:o"; + if (!m_plannedCoMPositionPort.open(plannedCoMPositionPortName)) + { + yError() << "[WalkingModule::configure] Could not open " << plannedCoMPositionPortName << " port."; + return false; + } + + m_runThreads = true; + m_virtualUnicyclePubliserThread = std::thread(&NavigationHelper::computeVirtualUnicycleThread, this, std::ref(FKSolver), std::ref(stableDCMModel), std::ref(trajectoryGenerator)); + return true; +} + +// This thread publishes the internal informations of the virtual unicycle extracted from the feet and the CoM speed +// It's the internal odometry +void NavigationHelper::computeVirtualUnicycleThread(std::unique_ptr &FKSolver, + std::unique_ptr &stableDCMModel, + std::unique_ptr &trajectoryGenerator + ) +{ + if (!m_publishInfo) + { //exit the funnction if the infos are not needed + return; + } + + yInfo() << "[NavigationHelper::computeVirtualUnicycleThread] Starting Thread"; + bool inContact = false; + while (m_runThreads) + { + //if (m_robotState != 4) + //{ + // std::this_thread::sleep_for(std::chrono::milliseconds(1000/m_navigationTriggerLoopRate)); + // continue; + //} + + iDynTree::Vector3 virtualUnicyclePose, virtualUnicycleReference; + std::string stanceFoot; + iDynTree::Transform footTransformToWorld, root_linkTransform; + //Stance foot check + if (m_leftInContact->size() > 0) + { + if (m_leftInContact->at(0)) + { + stanceFoot = "left"; + footTransformToWorld = FKSolver->getLeftFootToWorldTransform(); + } + else + { + stanceFoot = "right"; + footTransformToWorld = FKSolver->getRightFootToWorldTransform(); + } + inContact = true; + } + else if (m_rightInContact->size() > 0) + { + if (m_rightInContact->at(0)) + { + stanceFoot = "right"; + footTransformToWorld = FKSolver->getRightFootToWorldTransform(); + } + else + { + stanceFoot = "left"; + footTransformToWorld = FKSolver->getLeftFootToWorldTransform(); + } + inContact = true; + } + else + { + inContact = false; + } + root_linkTransform = FKSolver->getRootLinkToWorldTransform(); //world -> rootLink transform + //Data conversion and port writing + if (inContact) + { + if (trajectoryGenerator->getUnicycleState(stanceFoot, virtualUnicyclePose, virtualUnicycleReference)) + { + //send data + yarp::os::Stamp stamp (0, yarp::os::Time::now()); //move to private member of the class + m_unicyclePort.setEnvelope(stamp); + auto& data = m_unicyclePort.prepare(); + data.clear(); + auto& unicyclePose = data.addList(); + unicyclePose.addFloat64(virtualUnicyclePose(0)); + unicyclePose.addFloat64(virtualUnicyclePose(1)); + unicyclePose.addFloat64(virtualUnicyclePose(2)); + auto& referencePose = data.addList(); + referencePose.addFloat64(virtualUnicycleReference(0)); + referencePose.addFloat64(virtualUnicycleReference(1)); + referencePose.addFloat64(virtualUnicycleReference(2)); + data.addString(stanceFoot); + + //add root link stransform: X, Y, Z, r, p, yaw, + auto& transform = data.addList(); + transform.addFloat64(root_linkTransform.getPosition()(0)); + transform.addFloat64(root_linkTransform.getPosition()(1)); + transform.addFloat64(root_linkTransform.getPosition()(2)); + transform.addFloat64(root_linkTransform.getRotation().asRPY()(0)); + transform.addFloat64(root_linkTransform.getRotation().asRPY()(1)); + transform.addFloat64(root_linkTransform.getRotation().asRPY()(2)); + + //planned velocity of the CoM + auto comVel = stableDCMModel->getCoMVelocity(); + auto& velData = data.addList(); + velData.addFloat64(comVel(0)); + velData.addFloat64(comVel(1)); + m_unicyclePort.write(); + } + else + { + yError() << "[NavigationHelper::computeVirtualUnicycleThread] Could not getUnicycleState from m_trajectoryGenerator (no data sent)."; + } + } + std::this_thread::sleep_for(std::chrono::milliseconds(1000/m_navigationTriggerLoopRate)); + } + yInfo() << "[NavigationHelper::computeVirtualUnicycleThread] Terminated thread"; +} + +bool NavigationHelper::publishPlannedFootsteps(std::unique_ptr &trajectoryGenerator) +{ + if (!m_publishInfo) + { //exit the funnction if the infos are not needed + return true; + } + if (m_feetPort.isClosed()) + { + yError() << "[NavigationHelper::publishPlannedFootsteps] Feet port closed (no data sent)."; + return false; + } + + //Send footsteps info on port anyway (x, y, yaw) wrt root_link + std::vector leftFootprints, rightFootprints; + if (trajectoryGenerator->getFootprints(leftFootprints, rightFootprints)) + { + if (leftFootprints.size()>0 && rightFootprints.size()>0) + { + auto& feetData = m_feetPort.prepare(); + feetData.clear(); + auto& rightFeet = feetData.addList(); + auto& leftFeet = feetData.addList(); + //left foot + for (size_t i = 0; i < leftFootprints.size(); ++i) + { + auto& pose = leftFeet.addList(); + pose.addFloat64(leftFootprints[i].x); //x + pose.addFloat64(leftFootprints[i].y); //y + pose.addFloat64(leftFootprints[i].theta); //yaw + } + //right foot + for (size_t j = 0; j < rightFootprints.size(); ++j) + { + auto& pose = rightFeet.addList(); + pose.addFloat64(rightFootprints[j].x); //x + pose.addFloat64(rightFootprints[j].y); //y + pose.addFloat64(rightFootprints[j].theta); //yaw + } + m_feetPort.write(); + } + } + return true; +} + +bool NavigationHelper::publishCoM(bool newTrajectoryMerged, std::deque &m_DCMPositionDesired, + std::unique_ptr &m_stableDCMModel, + std::deque &m_leftTrajectory, + std::deque &m_rightTrajectory) +{ + if (!m_publishInfo) + { //exit the funnction if the infos are not needed + return true; + } + + // streaming CoM desired position and heading for Navigation algorithms + //if(!m_leftTrajectory.size() == m_DCMPositionDesired.size()) + //{ + // yWarning() << "[WalkingModule::updateModule] Inconsistent dimenstions. CoM planned poses will be augmented" ; + //} + + double yawLeftp, yawRightp, meanYawp; + yarp::sig::Vector& planned_poses = m_plannedCoMPositionPort.prepare(); + planned_poses.clear(); + + bool saveCoM = false; //flag to whether save the CoM trajectory only once each double support + if (m_leftInContact->size()>0 && m_rightInContact->size()>0) //consistency check + { + //evaluate the state of the contacts + if (m_leftInContact->at(0) && m_rightInContact->at(0)) //double support + { + if (!m_wasInDoubleSupport) + { + saveCoM = true; + m_wasInDoubleSupport = true; + } + else //I still need to assign a value to the flag + { + saveCoM = false; + } + + //override the previous state check if there has been a merge of a new trajectory on this thread cycle + //in this way we have the latest updated trajectory + if (newTrajectoryMerged) + { + saveCoM = true; + } + } + else + { + saveCoM = false; + if (m_wasInDoubleSupport) + { + m_wasInDoubleSupport = false; + } + } + } + + if (saveCoM) + { + m_desiredCoM_Trajectory.clear(); + } + for (auto i = 0; i < m_DCMPositionDesired.size(); i++) + { + m_stableDCMModel->setInput(m_DCMPositionDesired[i]); + m_stableDCMModel->integrateModel(); + yawLeftp = m_leftTrajectory[i].getRotation().asRPY()(2); + yawLeftp = m_rightTrajectory[i].getRotation().asRPY()(2); + meanYawp = std::atan2(std::sin(yawLeftp) + std::sin(yawRightp), + std::cos(yawLeftp) + std::cos(yawRightp)); + + planned_poses.push_back(m_stableDCMModel->getCoMPosition().data()[0]); + planned_poses.push_back(m_stableDCMModel->getCoMPosition().data()[1]); + planned_poses.push_back(meanYawp); + //saving data also internally -> should do this only once per double support + if (saveCoM) + { + iDynTree::Vector3 pose; + pose(0) = m_DCMPositionDesired[i](0); + pose(1) = m_DCMPositionDesired[i](1);; + pose(2) = meanYawp; + m_desiredCoM_Trajectory.push_back(pose); + } + } + //m_newTrajectoryMerged = false; //reset flag + m_plannedCoMPositionPort.write(); + m_stableDCMModel->reset(m_DCMPositionDesired.front()); + return true; +} \ No newline at end of file diff --git a/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h b/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h index 6ccd361c..4a4de061 100644 --- a/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h +++ b/src/TrajectoryPlanner/include/WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h @@ -264,6 +264,30 @@ namespace WalkingControllers * Reset the planner */ void reset(); + + struct Pose /**< Struct representing a pose in a plane 2.5D */ + { + double x; + double y; + double theta; + }; + + /** + * Get the footprints planned by the unicycle generator + * @param leftFootprints vector containing the desired poses of the left foot on the xy plane + * @param rightFootprints vector containing the desired poses of the right foot on the xy plane + * @return true/false in case of success/failure. + */ + bool getFootprints(std::vector& leftFootprints, std::vector& rightFootprints); + + /** + * Get the unicycle state from the feet of the robot in the odom frame (aka world frame) + * @param stanceFoot string with the stance foot + * @param virtualUnicyclePose pose of the virtual unicycle in the odom frame + * @param referenceUnicyclePose pose of the reference virtual unicycle in the odom frame + * @return true/false in case of success/failure. + */ + bool getUnicycleState(const std::string& stanceFoot, iDynTree::Vector3& virtualUnicyclePose, iDynTree::Vector3& referenceUnicyclePose); }; }; diff --git a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp index 9c95c490..a4106033 100644 --- a/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp +++ b/src/TrajectoryPlanner/src/TrajectoryGenerator.cpp @@ -117,6 +117,10 @@ bool TrajectoryGenerator::configurePlanner(const yarp::os::Searchable& config) yarp::os::Value(40.0)).asFloat64()); double minAngleVariation = iDynTree::deg2rad(config.check("minAngleVariation", yarp::os::Value(5.0)).asFloat64()); + if (minAngleVariation == 0.0) //configuration warning + { + yWarning() << "[configurePlanner] Setting minAngleVariation to 0.0 will make the robot jog in place in manual mode."; + } double maxStepDuration = config.check("maxStepDuration", yarp::os::Value(8.0)).asFloat64(); double minStepDuration = config.check("minStepDuration", yarp::os::Value(2.9)).asFloat64(); double stepHeight = config.check("stepHeight", yarp::os::Value(0.005)).asFloat64(); @@ -853,3 +857,103 @@ bool TrajectoryGenerator::getDesiredZMPPosition(std::vector & desiredZMP = m_dcmGenerator->getZMPPosition(); return true; } + +bool TrajectoryGenerator::getFootprints(std::vector& leftFootprints, std::vector& rightFootprints) +{ + auto leftDequeue = m_trajectoryGenerator.getLeftFootPrint()->getSteps(); + leftFootprints.resize(leftDequeue.size()); + for (size_t i = 0; i < leftDequeue.size(); ++i) + { + Pose pose; + pose.x = leftDequeue.at(i).position(0); + pose.y = leftDequeue.at(i).position(1); + pose.theta = leftDequeue.at(i).angle; + leftFootprints[i] = pose; + } + + auto rightDeque = m_trajectoryGenerator.getRightFootPrint()->getSteps(); + rightFootprints.resize(rightDeque.size()); + for (size_t i = 0; i < rightDeque.size(); ++i) + { + Pose pose; + pose.x = rightDeque.at(i).position(0); + pose.y = rightDeque.at(i).position(1); + pose.theta = rightDeque.at(i).angle; + rightFootprints[i] = pose; + } + + return true; +} + +bool TrajectoryGenerator::getUnicycleState(const std::string& stanceFoot, iDynTree::Vector3& virtualUnicyclePose, iDynTree::Vector3& referenceUnicyclePose) +{ + double nominalWidth; + bool correctLeft; + iDynTree::Vector2 measuredPositionLeft, measuredPositionRight; + iDynTree::Vector3 desiredDirectControl; + double measuredAngleLeft, measuredAngleRight; + double leftYawDeltaInRad, rightYawDeltaInRad; + // left foot + measuredPositionLeft(0) = m_measuredTransformLeft.getPosition()(0); + measuredPositionLeft(1) = m_measuredTransformLeft.getPosition()(1); + measuredAngleLeft = m_measuredTransformLeft.getRotation().asRPY()(2); + leftYawDeltaInRad = m_leftYawDeltaInRad; + // right foot + measuredPositionRight(0) = m_measuredTransformRight.getPosition()(0); + measuredPositionRight(1) = m_measuredTransformRight.getPosition()(1); + measuredAngleRight = m_measuredTransformRight.getRotation().asRPY()(2); + rightYawDeltaInRad = m_rightYawDeltaInRad; + correctLeft = m_correctLeft; + m_newFreeSpaceEllipse = false; + nominalWidth = m_nominalWidth; + + iDynTree::Vector2 measuredPosition; + double measuredAngle; + measuredPosition = correctLeft ? measuredPositionLeft : measuredPositionRight; + measuredAngle = correctLeft ? measuredAngleLeft : measuredAngleRight; + + Eigen::Vector2d unicyclePositionFromStanceFoot, footPosition, unicyclePosition, unicycleOdom; + unicyclePositionFromStanceFoot(0) = 0.0; + Eigen::Matrix2d unicycleRotation; //rotation world -> unicycle + double unicycleAngle, unicycleAngleOdom; + + if (stanceFoot == "left") + { + unicyclePositionFromStanceFoot(1) = -nominalWidth/2; + unicycleAngle = measuredAngleLeft; //- leftYawDeltaInRad + unicycleAngleOdom = measuredAngleLeft - leftYawDeltaInRad; + footPosition = iDynTree::toEigen(measuredPositionLeft); + } + else if (stanceFoot == "right") + { + unicyclePositionFromStanceFoot(1) = nominalWidth/2; + unicycleAngle = measuredAngleRight; //- rightYawDeltaInRad + unicycleAngleOdom = measuredAngleRight - rightYawDeltaInRad; + footPosition = iDynTree::toEigen(measuredPositionRight); + } + else + { + return false; + } + + //Odom (expressed in odom frame) + double s_theta = std::sin(unicycleAngleOdom); + double c_theta = std::cos(unicycleAngleOdom); + unicycleRotation(0,0) = c_theta; + unicycleRotation(0,1) = -s_theta; + unicycleRotation(1,0) = s_theta; + unicycleRotation(1,1) = c_theta; + unicyclePosition = unicycleRotation * unicyclePositionFromStanceFoot; // position in respect to the support foot + unicycleOdom = unicyclePosition + footPosition; //postion in odom frame + virtualUnicyclePose(0) = unicycleOdom(0); + virtualUnicyclePose(1) = unicycleOdom(1); + virtualUnicyclePose(2) = unicycleAngleOdom; + + iDynTree::Vector2 referencePointInAbsoluteFrame; + iDynTree::toEigen(referencePointInAbsoluteFrame) = unicycleRotation * (iDynTree::toEigen(m_referencePointDistance)) + unicycleOdom; + referenceUnicyclePose(0) = referencePointInAbsoluteFrame(0); + referenceUnicyclePose(1) = referencePointInAbsoluteFrame(1); + referenceUnicyclePose(2) = unicycleAngleOdom; + + return true; +} diff --git a/src/WalkingModule/CMakeLists.txt b/src/WalkingModule/CMakeLists.txt index a01fbf76..72ca89a2 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::NavigationHelper 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/ergoCubGazeboV1/dcm_walking/common/navigation.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/common/navigation.ini new file mode 100644 index 00000000..ce4d9053 --- /dev/null +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking/common/navigation.ini @@ -0,0 +1,11 @@ +# Loop rate for the thread computing the navigation trigger +navigationTriggerLoopRate 100 + +# Planner Mode: manual i.e. typical utilization of the planner for joystick use - navigation i.e. path following behavior +# If in NAVIGATION mode (1), the type of the controller () also specifies the behaviour of the planner: +# A- in direct mode the planner behaves like an omnidirectional floating base that interpolates the path. +# B- in personFollowing it tries to follow the path like an unicycle +plannerMode navigation + +# Flag that forces to publish the navigation required infos also in manual mode (default to 0) +publishNavigationInfo 1 \ No newline at end of file diff --git a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking_with_joypad.ini index 73717ec9..aa7ee84b 100644 --- a/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/ergoCubGazeboV1/dcm_walking_with_joypad.ini @@ -54,6 +54,9 @@ 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 root_link +# include navigation parameters +[include NAVIGATION "./dcm_walking/common/navigation.ini"] + # include robot control parameters [include ROBOT_CONTROL "./dcm_walking/joypad_control/robotControl.ini"] diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/common/navigation.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/common/navigation.ini new file mode 100644 index 00000000..f69e1c2f --- /dev/null +++ b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/common/navigation.ini @@ -0,0 +1,11 @@ +# Loop rate for the thread computing the navigation trigger +navigationTriggerLoopRate 100 + +# Planner Mode: manual i.e. typical utilization of the planner for joystick use - navigation i.e. path following behavior +# If in NAVIGATION mode (1), the type of the controller () also specifies the behaviour of the planner: +# A- in direct mode the planner behaves like an omnidirectional floating base that interpolates the path. +# B- in personFollowing it tries to follow the path like an unicycle +plannerMode direct + +# Flag that forces to publish the navigation required infos also in manual mode (default to 0) +publishNavigationInfo 1 \ No newline at end of file diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/common/plannerParams.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/common/plannerParams.ini index 9c1d3b60..89aa344b 100644 --- a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/common/plannerParams.ini +++ b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/common/plannerParams.ini @@ -32,7 +32,7 @@ maxAngleVariation 18.0 minAngleVariation 5.0 #Timings maxStepDuration 1.5 -minStepDuration 0.5 +minStepDuration 0.65 ##Nominal Values #Width diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/inverseKinematics.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/inverseKinematics.ini index 65a96c16..3118dc47 100644 --- a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/inverseKinematics.ini +++ b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/inverseKinematics.ini @@ -18,7 +18,7 @@ max-cpu-time 20 #DEGREES jointRegularization (0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, + 0.0, 0.0, 12.0, 7.0, -12.0, 41.0, -28.3, 0, 0, 12.0, 7.0, -12.0, 41.0, -28.3, 0, 0, 5.76, 1.61, -0.31, -31.64, -20.52, -1.52, diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini index 591b9a92..b2541be6 100644 --- a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini +++ b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/jointRetargeting.ini @@ -10,7 +10,7 @@ hde_port_name /humanState:i ## accordingly to the order of the joints received in the ## "joint_retargeting_port_name" port retargeting_joint_list ("neck_pitch", "neck_roll", "neck_yaw", - "torso_pitch", "torso_roll", "torso_yaw", + "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_roll", "l_wrist_pitch", "l_wrist_yaw", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_roll", "r_wrist_pitch", "r_wrist_yaw") diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/robotControl.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/robotControl.ini index d2ddab1b..804162ca 100644 --- a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/robotControl.ini +++ b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/robotControl.ini @@ -2,7 +2,7 @@ robot ergocub joints_list ("neck_pitch", "neck_roll", "neck_yaw", - "torso_pitch", "torso_roll", "torso_yaw", + "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_roll", "l_wrist_pitch", "l_wrist_yaw", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_roll", "r_wrist_pitch", "r_wrist_yaw", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", @@ -21,16 +21,16 @@ wrench_cut_frequency 10.0 # if true the joint is in stiff mode if false the joint is in compliant mode joint_is_stiff_mode (true, true, true, - true, true, true, - false, false, true, true, true, true, true, - false, false, true, true, true, true, true, + true, true, + true, true, true, true, true, true, true, + true, true, true, true, true, true, true, true, true, true, true, true, true, true, true, true, true, true, true) # if true a good joint traking is considered mandatory good_tracking_required (false, false, false - true, true, true, - false, false, true, true, false, false, false, - false, false, true, true, false, false, false, + true, true, + true, true, true, true, false, false, false, + true, true, true, true, false, false, false, true, true, true, true, true, true, true, true, true, true, true, true) diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini index 8611abc6..2a4e5378 100644 --- a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/tasks/regularization.ini @@ -1,6 +1,6 @@ robot_velocity_variable_name "robot_velocity" kp (5.0, 5.0, 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 @@ -13,7 +13,7 @@ settling_time 5.0 [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, @@ -22,14 +22,14 @@ weight (0.0, 0.0, 0.0, [WALKING] name "walking" # weight (0.0, 0.0, 0.0, -# 2.0, 2.0, 2.0, +# 2.0, 2.0, # 2.0, 2.0, 2.0, 0.0, 0.0, 0.0, 0.0, # 2.0, 2.0, 2.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) weight (0.0, 0.0, 0.0, - 2.0, 2.0, 2.0, + 2.0, 2.0, 0.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini index 45f04e11..669b1d2f 100644 --- a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini +++ b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/iFeel_joint_retargeting/tasks/retargeting.ini @@ -1,6 +1,6 @@ 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 @@ -13,7 +13,7 @@ settling_time 5.0 [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, 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, @@ -22,7 +22,7 @@ weight (2.0, 2.0, 2.0, [WALKING] name "walking" # weight (2.0, 2.0, 2.0, -# 0.0, 0.0, 0.0, +# 0.0, 0.0, # 0.0, 0.0, 0.0, 2.0, 2.0, 5.0, 5.0, # 0.0, 0.0, 0.0, 2.0, 2.0, 5.0, 5.0, # 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, @@ -30,7 +30,7 @@ name "walking" weight (2.0, 2.0, 2.0, - 0.0, 0.0, 0.0, + 0.0, 0.0, 2.0, 0.0, 2.0, 2.0, 2.0, 5.0, 5.0, 2.0, 0.0, 2.0, 2.0, 2.0, 5.0, 5.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/inverseKinematics.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/inverseKinematics.ini index 8ec7efc5..5091904f 100644 --- a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/inverseKinematics.ini +++ b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/inverseKinematics.ini @@ -31,7 +31,7 @@ max-cpu-time 20 # 5.76, 1.61, -0.31, -31.64, -20.52, -1.52, # 5.76, 1.61, -0.31, -31.64, -20.52, -1.52) -jointRegularization (7, 0.12, -0.01, +jointRegularization (0.12, -0.01, 12.0, 7.0, -12.0, 40.769, 12.0, 7.0, -12.0, 40.769, 5.76, 1.61, -0.31, -31.64, -20.52, -1.52, diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/robotControl.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/robotControl.ini index b80dc53b..cb720957 100644 --- a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/robotControl.ini +++ b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/robotControl.ini @@ -1,6 +1,6 @@ robot ergocub -joints_list ("torso_pitch", "torso_roll", "torso_yaw", +joints_list ("torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", @@ -18,15 +18,15 @@ wrench_cut_frequency 10.0 # if true the joint is in stiff mode if false the joint is in compliant mode -joint_is_stiff_mode (true, true, true, +joint_is_stiff_mode (true, true, true, true, true, true, true, true, true, true, true, true, true, true, true, true, true, true, true, true, true, true) # if true a good joint traking is considered mandatory -good_tracking_required (true, true, true, - true, true, true, false, - true, true, true, false, +good_tracking_required (true, true, + false, false, false, false, + false, false, false, false, true, true, true, true, true, true, true, true, true, true, true, true) diff --git a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/tasks/regularization.ini b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/tasks/regularization.ini index 4b75c831..f8ecdab2 100644 --- a/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/tasks/regularization.ini +++ b/src/WalkingModule/app/robots/ergoCubSN000/dcm_walking/joypad_control/tasks/regularization.ini @@ -1,5 +1,5 @@ robot_velocity_variable_name "robot_velocity" -kp (5.0, 5.0, 5.0, +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, @@ -11,7 +11,7 @@ settling_time 0.5 [stance] name "stance" -weight (1.0, 1.0, 1.0, +weight (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, @@ -19,7 +19,7 @@ weight (1.0, 1.0, 1.0, [walking] name "walking" -weight (1.0, 1.0, 1.0, +weight (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, diff --git a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h index 9c415cff..d235a533 100644 --- a/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h +++ b/src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h @@ -50,6 +50,8 @@ #include +#include + // iCub-ctrl #include @@ -123,6 +125,7 @@ namespace WalkingControllers std::deque m_comHeightVelocity; /**< Deque containing the CoM height velocity. */ std::deque m_mergePoints; /**< Deque containing the time position of the merge points. */ std::deque m_isStancePhase; /**< if true the robot is not walking */ + std::deque m_desiredCoM_Trajectory; /**< Deque containing the desired CoM trajectory projected on the ground in pose x, y, theta. */ std::deque m_isLeftFixedFrame; /**< Deque containing when the main frame of the left foot is the fixed frame In general a main frame of a foot is the fix frame only during the @@ -141,6 +144,7 @@ namespace WalkingControllers yarp::os::BufferedPort m_desiredUnyciclePositionPort; /**< Desired robot position port. */ bool m_newTrajectoryRequired; /**< if true a new trajectory will be merged soon. (after m_newTrajectoryMergeCounter - 2 cycles). */ + bool m_newTrajectoryMerged; /**< true if a new trajectory has been just merged. */ size_t m_newTrajectoryMergeCounter; /**< The new trajectory will be merged after m_newTrajectoryMergeCounter - 2 cycles. */ bool m_useRootLinkForHeight; @@ -155,6 +159,8 @@ namespace WalkingControllers size_t m_feedbackAttempts; double m_feedbackAttemptDelay; + NavigationHelper m_navHelper; + // debug std::unique_ptr m_velocityIntegral{nullptr}; diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 9b30624a..869dc341 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -11,6 +11,7 @@ #include #include #include +#include // YARP #include @@ -20,6 +21,8 @@ #include #include #include +#include +#include // iDynTree #include @@ -262,8 +265,11 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) m_trajectoryGenerator = std::make_unique(); yarp::os::Bottle& trajectoryPlannerOptions = rf.findGroup("TRAJECTORY_PLANNER"); yarp::os::Bottle ellipseMangerOptions = rf.findGroup("FREE_SPACE_ELLIPSE_MANAGER"); + yarp::os::Bottle navigationOptions = rf.findGroup("NAVIGATION"); trajectoryPlannerOptions.append(generalOptions); trajectoryPlannerOptions.append(ellipseMangerOptions); + trajectoryPlannerOptions.append(navigationOptions); + if(!m_trajectoryGenerator->initialize(trajectoryPlannerOptions)) { yError() << "[configure] Unable to initialize the planner."; @@ -401,7 +407,7 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) yError() << "[WalkingModule::configure] Failed to configure the retargeting"; return false; } - + // initialize the logger if(m_dumpData) { @@ -442,6 +448,12 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) m_qDesired.resize(m_robotControlHelper->getActuatedDoFs()); m_dqDesired.resize(m_robotControlHelper->getActuatedDoFs()); + + if(!m_navHelper.init(navigationOptions, m_leftInContact, m_rightInContact, m_FKSolver, m_stableDCMModel, m_trajectoryGenerator)) + { + yError() << "[WalkingModule::configure] Could not initialize the Navigation Helper"; + } + yInfo() << "[WalkingModule::configure] Ready to play! Please prepare the robot."; return true; @@ -470,6 +482,8 @@ bool WalkingModule::close() { if(m_dumpData) m_loggerPort.close(); + + m_navHelper.closeHelper(); // restore PID m_robotControlHelper->getPIDHandler().restorePIDs(); @@ -770,6 +784,7 @@ bool WalkingModule::updateModule() } m_newTrajectoryRequired = false; resetTrajectory = true; + m_newTrajectoryMerged = true; } m_newTrajectoryMergeCounter--; @@ -1021,6 +1036,12 @@ bool WalkingModule::updateModule() return false; } + //Send footsteps info on port anyway (x, y, yaw) wrt root_link + if(!m_navHelper.publishPlannedFootsteps(m_trajectoryGenerator)) + { + yWarning() << "[WalkingModule::updateModule] Unable to publish footsteps"; + } + // send data to the logger if(m_dumpData) {