diff --git a/include/mc_control/MCController.h b/include/mc_control/MCController.h index 33bcc136ea..4f48e125ee 100644 --- a/include/mc_control/MCController.h +++ b/include/mc_control/MCController.h @@ -17,6 +17,7 @@ #include #include +#include #include #include #include @@ -704,6 +705,8 @@ struct MC_CONTROL_DLLAPI MCController mc_rtc::unique_ptr compoundJointConstraint; /** Posture task for the main robot */ std::shared_ptr postureTask; + /** Coincidence constraints for the main robot */ + std::vector> coincidence_constraints_; /* Controller's name */ const std::string name_; /** Stores the loading location provided by the loader via \ref set_loading_location */ @@ -734,6 +737,8 @@ struct MC_CONTROL_DLLAPI MCController * The value is stored in a thread_local variable and is meant to be used in the constructor of MCController */ static void set_name(std::string_view name); + + void loop_closing(const mc_rbdyn::Robot & robot); }; namespace details diff --git a/include/mc_rbdyn/RobotModule.h b/include/mc_rbdyn/RobotModule.h index 526447f1f9..b27af7af65 100644 --- a/include/mc_rbdyn/RobotModule.h +++ b/include/mc_rbdyn/RobotModule.h @@ -26,6 +26,7 @@ #include #include #include +#include /* This is an interface designed to provide additionnal information about a robot */ @@ -273,7 +274,7 @@ struct MC_RBDYN_DLLAPI RobotModule * - Create a default joint order * - Create a default stance */ - void init(const rbd::parsers::ParserResult & res); + void init(const rbd::parsers::ParserResult & res, const YAML::Node & res_yaml = {}); /** Returns the robot's bounds obtained from parsing a urdf * @@ -559,6 +560,8 @@ struct MC_RBDYN_DLLAPI RobotModule std::string name; /** Path to the robot's URDF file */ std::string urdf_path; + /** Path to the robot's URDF file */ + std::string yaml_path; /** Path to the robot's RSDF folder */ std::string rsdf_dir; /** Path to the robot's calib folder */ @@ -627,6 +630,11 @@ struct MC_RBDYN_DLLAPI RobotModule DevicePtrVector _devices; /** \see frames() */ std::vector _frames; + + // Closed loop variables + std::vector> link_names_; + std::vector types_; + std::vector> motors_; }; typedef std::shared_ptr RobotModulePtr; diff --git a/include/mc_solver/CoincidenceConstr.h b/include/mc_solver/CoincidenceConstr.h new file mode 100644 index 0000000000..ced69c6338 --- /dev/null +++ b/include/mc_solver/CoincidenceConstr.h @@ -0,0 +1,57 @@ +#pragma once + +#include +#include +#include + +namespace mc_rbdyn +{ +struct Robots; +} + +namespace mc_solver +{ + +struct QPSolver; + +/** \class CoincidenceConstraint + * + * Constraint to maintain coincidence between two points on a robot + */ +struct MC_SOLVER_DLLAPI CoincidenceConstraint : public ConstraintSet +{ +public: + /** Constructor + * + * \param robots The robots for which the constraint will apply + * \param name1 Name of the first link + * \param name1 Name of the second link + * \param type Type of the joint + * \param dt Time step of the control + */ + CoincidenceConstraint(const mc_rbdyn::Robots & robots, + std::string name1, + std::string name2, + std::string type, + const Eigen::VectorXd & joints, + double dt); + + void addToSolverImpl(QPSolver & solver) override; + + void removeFromSolverImpl(QPSolver & solver) override; + +public: + /** Holds the constraint implementation */ + std::string name1_; + std::string name2_; + std::string type_; + Eigen::VectorXd joints_; + double dt_; + + tasks::qp::CoincidenceConstr * coincidenceConstr(); + +private: + mc_rtc::void_ptr constraint_; +}; + +} // namespace mc_solver diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 34a54c1cd5..da1ac92191 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -432,6 +432,7 @@ set(mc_solver_SRC mc_solver/Update.cpp mc_solver/UpdateNrVars.cpp mc_tasks/MetaTask.cpp + mc_solver/CoincidenceConstr.cpp ) set(mc_solver_HDR @@ -459,6 +460,7 @@ set(mc_solver_HDR ../include/mc_solver/TasksQPSolver.h ../include/mc_solver/TVMQPSolver.h ../include/mc_tasks/MetaTask.h + ../include/mc_solver/CoincidenceConstr.h ) add_library(mc_solver SHARED ${mc_solver_SRC} ${mc_solver_HDR}) diff --git a/src/mc_control/MCController.cpp b/src/mc_control/MCController.cpp index 5c41518c2b..51e0b82dde 100644 --- a/src/mc_control/MCController.cpp +++ b/src/mc_control/MCController.cpp @@ -27,6 +27,9 @@ #include #include +#include "mc_solver/CoincidenceConstr.h" +#include +#include namespace bfs = boost::filesystem; #include @@ -311,6 +314,10 @@ MCController::MCController(const std::vector motors = robot.module().motors_[n]; + Eigen::VectorXd joints_vec = Eigen::VectorXd::Zero(robot.mb().nrDof()); + int idx = 5; // we do not considerate the fisrt 6 values of the body pos + for(int i = 0; i < std::size(robot.mbc().alphaD); i++) + { + if(robot.mb().joint(i).dof() >= 1) + { + auto it = std::find(motors.begin(), motors.end(), robot.mb().joint(i).name()); + if(it != motors.end()) { joints_vec[idx] = 1.0; } + idx++; + } + } + + auto frame_pair = robot.module().link_names_[n]; + std::string name1 = frame_pair[0]; + std::string name2 = frame_pair[1]; + std::string type = robot.module().types_[n]; + auto coincidence_constraint = + std::make_shared(robots(), name1, name2, type, joints_vec, solver().dt()); + coincidence_constraints_.push_back(coincidence_constraint); + solver().addConstraintSet(*coincidence_constraint); + } + } + catch(const std::exception & e) + { + mc_rtc::log::error("Failed to load constraints: {}", e.what()); + } +} + } // namespace mc_control diff --git a/src/mc_rbdyn/RobotLoader.cpp b/src/mc_rbdyn/RobotLoader.cpp index 13b65e8559..2b1c6a7431 100644 --- a/src/mc_rbdyn/RobotLoader.cpp +++ b/src/mc_rbdyn/RobotLoader.cpp @@ -2,6 +2,8 @@ * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL */ +#include + #include #include diff --git a/src/mc_rbdyn/RobotModule.cpp b/src/mc_rbdyn/RobotModule.cpp index 3551bf2f62..5720dff95a 100644 --- a/src/mc_rbdyn/RobotModule.cpp +++ b/src/mc_rbdyn/RobotModule.cpp @@ -37,7 +37,7 @@ RobotModule::RobotModule(const std::string & name, const rbd::parsers::ParserRes init(res); } -void RobotModule::init(const rbd::parsers::ParserResult & res) +void RobotModule::init(const rbd::parsers::ParserResult & res, const YAML::Node & res_yaml) { mb = res.mb; mbc = res.mbc; @@ -53,6 +53,17 @@ void RobotModule::init(const rbd::parsers::ParserResult & res) _collision = res.collision; if(_ref_joint_order.size() == 0) { make_default_ref_joint_order(); } expand_stance(); + + if(res_yaml) + { + if(res_yaml["name_mot"] && res_yaml["closed_loop"] && res_yaml["type"] && !res_yaml["name_mot"].IsNull() + && !res_yaml["closed_loop"].IsNull() && !res_yaml["type"].IsNull()) + { + link_names_ = res_yaml["closed_loop"].as>>(); + types_ = res_yaml["type"].as>(); + motors_ = res_yaml["name_mot"].as>>(); + } + } } RobotModule::Gripper::Gripper(const std::string & name, const std::vector & joints, bool reverse_limits) diff --git a/src/mc_solver/CoincidenceConstr.cpp b/src/mc_solver/CoincidenceConstr.cpp new file mode 100644 index 0000000000..5889b70faf --- /dev/null +++ b/src/mc_solver/CoincidenceConstr.cpp @@ -0,0 +1,97 @@ +/* + * Copyright 2015-2022 CNRS-UM LIRMM, CNRS-AIST JRL + */ + +#include + +#include + +#include +#include +#include +#include + +#include +#include + +#include + +namespace mc_solver +{ + +static mc_rtc::void_ptr make_constraint(QPSolver::Backend backend, + int robot_index, + const std::string & name1, + const std::string & name2, + const Eigen::VectorXd & joints, + const std::string & type) +{ + switch(backend) + { + case QPSolver::Backend::Tasks: + { + if(type == "6d") + { + return mc_rtc::make_void_ptr(robot_index, name1, name2, joints); + } + else if(type == "3d") + { + return mc_rtc::make_void_ptr(robot_index, name1, name2, joints); + } + else { mc_rtc::log::error_and_throw("[CoincidenceConstraint] Invalid coincidence type given to constructor"); } + } + case QPSolver::Backend::TVM: + return {nullptr, nullptr}; + default: + mc_rtc::log::error_and_throw("[CoincidenceConstraint] Not implemented for solver backend: {}", backend); + } +} + +CoincidenceConstraint::CoincidenceConstraint(const mc_rbdyn::Robots & robots, + std::string name1, + std::string name2, + std::string type, + const Eigen::VectorXd & joints, + double dt) +: ConstraintSet(), name1_(name1), name2_(name2), type_(type), joints_(joints), dt_(dt), + constraint_(make_constraint(backend_, robots.robotIndex(), name1_, name2_, joints_, type_)) +{ +} + +void CoincidenceConstraint::addToSolverImpl(QPSolver & solver) +{ + switch(backend_) + { + case QPSolver::Backend::Tasks: + static_cast(constraint_.get()) + ->addToSolver(solver.robots().mbs(), tasks_solver(solver).solver()); + mc_rtc::log::success("CoincidenceConstraint {} <-> {} added successfully!", name1_, name2_); + break; + case QPSolver::Backend::TVM: + break; + } +} + +void CoincidenceConstraint::removeFromSolverImpl(QPSolver & solver) +{ + switch(backend_) + { + case QPSolver::Backend::Tasks: + static_cast(constraint_.get())->removeFromSolver(tasks_solver(solver).solver()); + mc_rtc::log::info("CoincidenceConstraint {} <-> {} removed", name1_, name2_); + break; + case QPSolver::Backend::TVM: + break; + } +} + +tasks::qp::CoincidenceConstr * CoincidenceConstraint::coincidenceConstr() +{ + if(backend_ != QPSolver::Backend::Tasks) + { + mc_rtc::log::error_and_throw("[CoincidenceConstraint] coincidenceConstr() is only usable for the Tasks backend"); + } + return static_cast(constraint_.get()); +} + +} // namespace mc_solver diff --git a/src/mc_solver/ConstraintSet.cpp b/src/mc_solver/ConstraintSet.cpp index 82e37684cc..ab5a2ae65b 100644 --- a/src/mc_solver/ConstraintSet.cpp +++ b/src/mc_solver/ConstraintSet.cpp @@ -5,6 +5,7 @@ #include #include +#include namespace mc_solver { diff --git a/src/mc_solver/ContactConstraint.cpp b/src/mc_solver/ContactConstraint.cpp index 58ca29e121..3992b1af2e 100644 --- a/src/mc_solver/ContactConstraint.cpp +++ b/src/mc_solver/ContactConstraint.cpp @@ -11,6 +11,7 @@ #include #include +#include namespace mc_solver {