diff --git a/include/mc_control/ControllerClient.h b/include/mc_control/ControllerClient.h index ac969a23dd..91ab79c5a3 100644 --- a/include/mc_control/ControllerClient.h +++ b/include/mc_control/ControllerClient.h @@ -185,7 +185,7 @@ struct MC_CONTROL_CLIENT_DLLAPI ControllerClient virtual void stopped() {} /** Should be implemented to create a new category container */ - virtual void category(const std::vector & parent, const std::string & category){}; + virtual void category(const std::vector & parent, const std::string & category) {}; /** Should be implemented to create a label for data that can be displayed as string */ inline virtual void label(const ElementId & id, const std::string &) { default_impl("Label", id); } diff --git a/include/mc_rbdyn/gui/RobotSurface.h b/include/mc_rbdyn/gui/RobotSurface.h index 242901d8b0..8f66ccf00f 100644 --- a/include/mc_rbdyn/gui/RobotSurface.h +++ b/include/mc_rbdyn/gui/RobotSurface.h @@ -10,9 +10,8 @@ namespace mc_rbdyn::gui { -static const mc_rtc::gui::LineConfig defaultSurfaceConfig = []() { - return mc_rtc::gui::LineConfig{mc_rtc::gui::Color::Green, 0.01}; -}(); +static const mc_rtc::gui::LineConfig defaultSurfaceConfig = []() +{ return mc_rtc::gui::LineConfig{mc_rtc::gui::Color::Green, 0.01}; }(); /** Helper function to create a GUI element from a surface object inside a robot * diff --git a/include/mc_rtc/gui/plot/AbscissaOrdinate.h b/include/mc_rtc/gui/plot/AbscissaOrdinate.h index 18c80a22af..f49f72669a 100644 --- a/include/mc_rtc/gui/plot/AbscissaOrdinate.h +++ b/include/mc_rtc/gui/plot/AbscissaOrdinate.h @@ -129,21 +129,15 @@ auto XY(std::string_view name, if constexpr(std::is_same_v, Color>) { return impl::AbscissaOrdinate( - name, - [get_x_fn, get_y_fn](XYCacheT & cache) { - cache.push_back({get_x_fn(), get_y_fn()}); - }, - color, style, side); + name, [get_x_fn, get_y_fn](XYCacheT & cache) { cache.push_back({get_x_fn(), get_y_fn()}); }, color, style, + side); } else { static_assert(details::CheckReturnType::value, "XY color callback should return a color"); return impl::AbscissaOrdinateWithColor( - name, - [get_x_fn, get_y_fn](XYCacheT & cache) { - cache.push_back({get_x_fn(), get_y_fn()}); - }, - color, style, side); + name, [get_x_fn, get_y_fn](XYCacheT & cache) { cache.push_back({get_x_fn(), get_y_fn()}); }, color, style, + side); } } diff --git a/include/mc_rtc/log/Logger.h b/include/mc_rtc/log/Logger.h index 8232b95e0c..877f6d626a 100644 --- a/include/mc_rtc/log/Logger.h +++ b/include/mc_rtc/log/Logger.h @@ -221,9 +221,8 @@ struct MC_RTC_UTILS_DLLAPI Logger } auto log_type = log::callback_is_serializable::log_type; log_events_.push_back(KeyAddedEvent{log_type, name}); - log_entries_.push_back({log_type, name, source, [get_fn](mc_rtc::MessagePackBuilder & builder) mutable { - mc_rtc::log::LogWriter::write(get_fn(), builder); - }}); + log_entries_.push_back({log_type, name, source, [get_fn](mc_rtc::MessagePackBuilder & builder) mutable + { mc_rtc::log::LogWriter::write(get_fn(), builder); }}); } /** Add a log entry from a source and a compile-time pointer to member @@ -248,8 +247,7 @@ struct MC_RTC_UTILS_DLLAPI Logger void addLogEntry(const std::string & name, const SourceT * source, bool overwrite = false) { using MemberT = decltype(source->*member); - addLogEntry( - name, source, [source]() -> const MemberT & { return source->*member; }, overwrite); + addLogEntry(name, source, [source]() -> const MemberT & { return source->*member; }, overwrite); } /** Add a log entry from a source and a compile-time pointer to method @@ -274,8 +272,7 @@ struct MC_RTC_UTILS_DLLAPI Logger void addLogEntry(const std::string & name, const SourceT * source, bool overwrite = false) { using MethodRetT = decltype((source->*method)()); - addLogEntry( - name, source, [source]() -> MethodRetT { return (source->*method)(); }, overwrite); + addLogEntry(name, source, [source]() -> MethodRetT { return (source->*method)(); }, overwrite); } /** Add a log entry into the log with no source diff --git a/include/mc_rtc/log/iterate_binary_log.h b/include/mc_rtc/log/iterate_binary_log.h index 71dd2c918d..8e72ce5ae1 100644 --- a/include/mc_rtc/log/iterate_binary_log.h +++ b/include/mc_rtc/log/iterate_binary_log.h @@ -81,9 +81,8 @@ inline bool iterate_binary_log(const std::string & fpath, const std::string & time = "t") { return iterate_binary_log( - fpath, - [&callback](IterateBinaryLogData data) { return callback(data.keys, data.records, data.time.value_or(-1)); }, - extract, time); + fpath, [&callback](IterateBinaryLogData data) + { return callback(data.keys, data.records, data.time.value_or(-1)); }, extract, time); } } // namespace mc_rtc::log diff --git a/include/mc_tasks/SplineTrajectoryTask.hpp b/include/mc_tasks/SplineTrajectoryTask.hpp index 9573da96da..4a4e8a2cfb 100644 --- a/include/mc_tasks/SplineTrajectoryTask.hpp +++ b/include/mc_tasks/SplineTrajectoryTask.hpp @@ -383,8 +383,8 @@ void SplineTrajectoryTask::addToGUI(mc_rtc::gui::StateBuilder & gui) { TrajectoryTask::addToGUI(gui); - gui.addElement({"Tasks", name_}, mc_rtc::gui::Checkbox( - "Paused", [this]() { return paused_; }, [this]() { paused_ = !paused_; })); + gui.addElement({"Tasks", name_}, + mc_rtc::gui::Checkbox("Paused", [this]() { return paused_; }, [this]() { paused_ = !paused_; })); gui.addElement({"Tasks", name_}, mc_rtc::gui::Transform("Surface pose", [this]() { return frame_->position(); })); gui.addElement({"Tasks", name_}, mc_rtc::gui::Rotation( diff --git a/plugins/ROS/src/mc_tasks_ros/LookAtTFTask.cpp b/plugins/ROS/src/mc_tasks_ros/LookAtTFTask.cpp index a1078ce3b5..23627541aa 100644 --- a/plugins/ROS/src/mc_tasks_ros/LookAtTFTask.cpp +++ b/plugins/ROS/src/mc_tasks_ros/LookAtTFTask.cpp @@ -103,4 +103,4 @@ static auto registered = mc_tasks::MetaTaskLoader::register_load_function( t->load(solver, config); return t; }); -} +} // namespace diff --git a/src/mc_control/MCController.cpp b/src/mc_control/MCController.cpp index e4aa3fe7bf..f05e3b3154 100644 --- a/src/mc_control/MCController.cpp +++ b/src/mc_control/MCController.cpp @@ -415,9 +415,8 @@ void MCController::addRobotToGUI(const mc_rbdyn::Robot & r) data("joints").add(r.name(), r.module().ref_joint_order()); data("frames").add(r.name(), r.frames()); auto name = r.name(); - gui()->addElement( - {"Robots"}, - mc_rtc::gui::Robot(r.name(), [name, this]() -> const mc_rbdyn::Robot & { return this->outputRobot(name); })); + gui()->addElement({"Robots"}, mc_rtc::gui::Robot(r.name(), [name, this]() -> const mc_rbdyn::Robot & + { return this->outputRobot(name); })); } void MCController::addRobotToLog(const mc_rbdyn::Robot & r) @@ -493,16 +492,14 @@ void MCController::addRobotToLog(const mc_rbdyn::Robot & r) { logger().addLogEntry(entry("ff"), [this, name]() -> const sva::PTransformd & { return outputRobot(name).mbc().bodyPosW[0]; }); - logger().addLogEntry(entry("ff_real"), - [this, name]() -> const sva::PTransformd & + logger().addLogEntry(entry("ff_real"), [this, name]() -> const sva::PTransformd & { return outputRealRobot(name).mbc().bodyPosW[0]; }); } // Log all force sensors for(const auto & fs : robot(name).forceSensors()) { const auto & fs_name = fs.name(); - logger().addLogEntry(entry_str(fs.name()), - [this, name, fs_name]() -> const sva::ForceVecd & + logger().addLogEntry(entry_str(fs.name()), [this, name, fs_name]() -> const sva::ForceVecd & { return robot(name).forceSensor(fs_name).wrench(); }); } // Log all body sensors @@ -510,23 +507,17 @@ void MCController::addRobotToLog(const mc_rbdyn::Robot & r) for(size_t i = 0; i < bodySensors.size(); ++i) { const auto & bs_name = bodySensors[i].name(); - logger().addLogEntry(entry_str(bs_name + "_position"), - [this, name, bs_name]() -> const Eigen::Vector3d & + logger().addLogEntry(entry_str(bs_name + "_position"), [this, name, bs_name]() -> const Eigen::Vector3d & { return robot(name).bodySensor(bs_name).position(); }); - logger().addLogEntry(entry_str(bs_name + "_orientation"), - [this, name, bs_name]() -> const Eigen::Quaterniond & + logger().addLogEntry(entry_str(bs_name + "_orientation"), [this, name, bs_name]() -> const Eigen::Quaterniond & { return robot(name).bodySensor(bs_name).orientation(); }); - logger().addLogEntry(entry_str(bs_name + "_linearVelocity"), - [this, name, bs_name]() -> const Eigen::Vector3d & + logger().addLogEntry(entry_str(bs_name + "_linearVelocity"), [this, name, bs_name]() -> const Eigen::Vector3d & { return robot(name).bodySensor(bs_name).linearVelocity(); }); - logger().addLogEntry(entry_str(bs_name + "_angularVelocity"), - [this, name, bs_name]() -> const Eigen::Vector3d & + logger().addLogEntry(entry_str(bs_name + "_angularVelocity"), [this, name, bs_name]() -> const Eigen::Vector3d & { return robot(name).bodySensor(bs_name).angularVelocity(); }); - logger().addLogEntry(entry_str(bs_name + "_linearAcceleration"), - [this, name, bs_name]() -> const Eigen::Vector3d & + logger().addLogEntry(entry_str(bs_name + "_linearAcceleration"), [this, name, bs_name]() -> const Eigen::Vector3d & { return robot(name).bodySensor(bs_name).linearAcceleration(); }); - logger().addLogEntry(entry_str(bs_name + "_angularAcceleration"), - [this, name, bs_name]() -> const Eigen::Vector3d & + logger().addLogEntry(entry_str(bs_name + "_angularAcceleration"), [this, name, bs_name]() -> const Eigen::Vector3d & { return robot(name).bodySensor(bs_name).angularAcceleration(); }); } // Log all joint sensors @@ -766,8 +757,7 @@ void MCController::reset(const ControllerResetData & reset_data) mc_rtc::gui::FormNumberInput("Friction", false, mc_rbdyn::Contact::defaultFriction), mc_rtc::gui::FormArrayInput("dof", false, Eigen::Vector6d::Ones()))); } - logger().addLogEntry( - "perf_UpdateContacts", [this]() { return updateContacts_dt_.count(); }, true); + logger().addLogEntry("perf_UpdateContacts", [this]() { return updateContacts_dt_.count(); }, true); } void MCController::updateContacts() diff --git a/src/mc_control/fsm/Executor.cpp b/src/mc_control/fsm/Executor.cpp index 7cff509ce4..a8034f0016 100644 --- a/src/mc_control/fsm/Executor.cpp +++ b/src/mc_control/fsm/Executor.cpp @@ -63,8 +63,7 @@ void Executor::init(Controller & ctl, else { log_entry += "_Main"; } ctl.logger().addLogEntry(log_entry, this, [this]() { return curr_state_; }); log_entry = "perf_" + log_entry; - ctl.logger().addLogEntry(log_entry, this, - [this]() + ctl.logger().addLogEntry(log_entry, this, [this]() { return state_create_dt_.count() + state_run_dt_.count() + state_teardown_dt_.count(); }); ctl.logger().addLogEntry(log_entry + "_create", this, [this]() { return state_create_dt_.count(); }); ctl.logger().addLogEntry(log_entry + "_run", this, [this]() { return state_run_dt_.count(); }); diff --git a/src/mc_control/fsm/states/Message.cpp b/src/mc_control/fsm/states/Message.cpp index 45be2a04e2..0d0c6af310 100644 --- a/src/mc_control/fsm/states/Message.cpp +++ b/src/mc_control/fsm/states/Message.cpp @@ -39,9 +39,7 @@ void MessageState::start(Controller & ctl) else if(logType_ == "success") { mc_rtc::log::success(message); } else if(logType_ == "warning") { mc_rtc::log::warning(message); } else if(logType_ == "error") { mc_rtc::log::error(message); } - else if(logType_ == "none") - { /* Do not log anything to the terminal */ - } + else if(logType_ == "none") { /* Do not log anything to the terminal */ } else { mc_rtc::log::error("{} Provided log type {} is invalid, assuming info. Supported types are [info, success, " diff --git a/src/mc_control/mc_global_controller_gui.cpp b/src/mc_control/mc_global_controller_gui.cpp index 9fa47c3dd2..ea8c16f9f3 100644 --- a/src/mc_control/mc_global_controller_gui.cpp +++ b/src/mc_control/mc_global_controller_gui.cpp @@ -43,11 +43,10 @@ void MCGlobalController::initGUI() targetCat.push_back("Targets"); for(const auto & joint : g_ptr->activeJoints()) { - gui->addElement(targetCat, - mc_rtc::gui::NumberSlider( - joint, [joint, g_ptr]() { return g_ptr->curOpening(joint); }, - [joint, g_ptr](double targetOpening) { g_ptr->setTargetOpening(joint, targetOpening); }, - 0, 1)); + gui->addElement(targetCat, mc_rtc::gui::NumberSlider( + joint, [joint, g_ptr]() { return g_ptr->curOpening(joint); }, + [joint, g_ptr](double targetOpening) + { g_ptr->setTargetOpening(joint, targetOpening); }, 0, 1)); } category.push_back("Safety"); diff --git a/src/mc_observers/EncoderObserver.cpp b/src/mc_observers/EncoderObserver.cpp index 9fc11e148c..6076872ecc 100644 --- a/src/mc_observers/EncoderObserver.cpp +++ b/src/mc_observers/EncoderObserver.cpp @@ -173,8 +173,7 @@ void EncoderObserver::addToLogger(const mc_control::MCController & ctl, } else if(velUpdate_ == VelUpdate::EncoderVelocities) { - logger.addLogEntry(category + "_encoderVelocities", this, - [this, &ctl]() -> const std::vector & + logger.addLogEntry(category + "_encoderVelocities", this, [this, &ctl]() -> const std::vector & { return ctl.robot(robot_).encoderVelocities(); }); } else if(velUpdate_ == VelUpdate::Control) diff --git a/src/mc_rbdyn/gui/RobotSurface.cpp b/src/mc_rbdyn/gui/RobotSurface.cpp index 0bac2487be..c4ae438814 100644 --- a/src/mc_rbdyn/gui/RobotSurface.cpp +++ b/src/mc_rbdyn/gui/RobotSurface.cpp @@ -26,11 +26,8 @@ std::vector addSurfaceToGUI(mc_rtc::gui::StateBuilder & gui, { const auto & cylinder = dynamic_cast(surface); publish_surface(mc_rtc::gui::Cylinder( - publishName.value_or(name), - [&cylinder]() { - return mc_rtc::gui::CylinderParameters{cylinder.radius(), cylinder.width()}; - }, - get_pose, cfg.color)); + publishName.value_or(name), [&cylinder]() + { return mc_rtc::gui::CylinderParameters{cylinder.radius(), cylinder.width()}; }, get_pose, cfg.color)); } else if(surface.type() == "planar") { diff --git a/src/mc_robots/json.cpp b/src/mc_robots/json.cpp index bc206ff727..509896aea0 100644 --- a/src/mc_robots/json.cpp +++ b/src/mc_robots/json.cpp @@ -45,6 +45,6 @@ static auto registered = []() mc_rbdyn::RobotLoader::register_object("json", callback); return true; }(); -} +} // namespace #endif diff --git a/src/mc_robots/jvrc1.cpp b/src/mc_robots/jvrc1.cpp index 635098db0f..c9118cdc0f 100644 --- a/src/mc_robots/jvrc1.cpp +++ b/src/mc_robots/jvrc1.cpp @@ -177,6 +177,6 @@ static auto registered = []() mc_rbdyn::RobotLoader::register_object("JVRC1Fixed", fn_t([]() { return new mc_robots::JVRC1RobotModule(true); })); return true; }(); -} +} // namespace #endif diff --git a/src/mc_rtc/iterate_binary_log.cpp b/src/mc_rtc/iterate_binary_log.cpp index c47bf698ac..dd318f33c9 100644 --- a/src/mc_rtc/iterate_binary_log.cpp +++ b/src/mc_rtc/iterate_binary_log.cpp @@ -90,10 +90,10 @@ bool iterate_binary_log(const std::string & f, } return keys_str; }(); - if(!callback(IterateBinaryLogData{keys_str, log.records(), events, t, - [&log](mc_rtc::MessagePackBuilder & builder, - const std::vector & keys) { log.copy(builder, keys); }, - buffer.data(), entrySize, meta})) + if(!callback( + IterateBinaryLogData{keys_str, log.records(), events, t, + [&log](mc_rtc::MessagePackBuilder & builder, const std::vector & keys) + { log.copy(builder, keys); }, buffer.data(), entrySize, meta})) { return false; } diff --git a/src/mc_rtc/loader.in.cpp b/src/mc_rtc/loader.in.cpp index ca8803e402..f3e8b75b72 100644 --- a/src/mc_rtc/loader.in.cpp +++ b/src/mc_rtc/loader.in.cpp @@ -212,8 +212,7 @@ void Loader::load_libraries(const std::string & class_name, std::vector drange; std::copy(dit, endit, std::back_inserter(drange)); // Sort by newest file - std::sort(drange.begin(), drange.end(), - [](const bfs::path & p1, const bfs::path & p2) + std::sort(drange.begin(), drange.end(), [](const bfs::path & p1, const bfs::path & p2) { return bfs::last_write_time(p1) > bfs::last_write_time(p2); }); for(const auto & p : drange) { diff --git a/src/mc_solver/BoundedSpeedConstr.cpp b/src/mc_solver/BoundedSpeedConstr.cpp index 207c017071..5099d43694 100644 --- a/src/mc_solver/BoundedSpeedConstr.cpp +++ b/src/mc_solver/BoundedSpeedConstr.cpp @@ -42,11 +42,9 @@ struct TVMBoundedSpeedConstr std::vector::iterator getData(const mc_rbdyn::RobotFrame & frame) { - return std::find_if(data_.begin(), data_.end(), - [&](const auto & d) { - return d.fn->frame().name() == frame.name() - && d.fn->frame().robot().name() == frame.robot().name(); - }); + return std::find_if( + data_.begin(), data_.end(), [&](const auto & d) + { return d.fn->frame().name() == frame.name() && d.fn->frame().robot().name() == frame.robot().name(); }); } void addBoundedSpeed(TVMQPSolver * solver, @@ -391,4 +389,4 @@ static auto registered = mc_solver::ConstraintSetLoader::register_load_function( } return ret; }); -} +} // namespace diff --git a/src/mc_solver/CoMIncPlaneConstr.cpp b/src/mc_solver/CoMIncPlaneConstr.cpp index d63c418a78..8ef9f7957a 100644 --- a/src/mc_solver/CoMIncPlaneConstr.cpp +++ b/src/mc_solver/CoMIncPlaneConstr.cpp @@ -277,4 +277,4 @@ static auto registered = mc_solver::ConstraintSetLoader::register_load_function( solver.robots(), robotIndexFromConfig(config, solver.robots(), "CoMIncPlane"), solver.dt()); return ret; }); -} +} // namespace diff --git a/src/mc_solver/CollisionsConstraint.cpp b/src/mc_solver/CollisionsConstraint.cpp index ce792f467f..3c44d1ab43 100644 --- a/src/mc_solver/CollisionsConstraint.cpp +++ b/src/mc_solver/CollisionsConstraint.cpp @@ -629,4 +629,4 @@ static auto registered = mc_solver::ConstraintSetLoader::register_load_function( ret->addCollisions(solver, collisions); return ret; }); -} +} // namespace diff --git a/src/mc_solver/CompoundJointConstraint.cpp b/src/mc_solver/CompoundJointConstraint.cpp index 4e0117f86a..240fb271cf 100644 --- a/src/mc_solver/CompoundJointConstraint.cpp +++ b/src/mc_solver/CompoundJointConstraint.cpp @@ -208,4 +208,4 @@ static auto registered = mc_solver::ConstraintSetLoader::register_load_function( } else { return std::make_shared(solver.robots(), rIndex, solver.dt()); } }); -} +} // namespace diff --git a/src/mc_solver/ContactConstraint.cpp b/src/mc_solver/ContactConstraint.cpp index c1dbfc37ff..45ca2459ff 100644 --- a/src/mc_solver/ContactConstraint.cpp +++ b/src/mc_solver/ContactConstraint.cpp @@ -106,4 +106,4 @@ static auto registered = mc_solver::ConstraintSetLoader::register_load_function( } return std::make_shared(solver.dt(), cType); }); -} +} // namespace diff --git a/src/mc_solver/TVMQPSolver.cpp b/src/mc_solver/TVMQPSolver.cpp index 5b986b7883..562771e12f 100644 --- a/src/mc_solver/TVMQPSolver.cpp +++ b/src/mc_solver/TVMQPSolver.cpp @@ -390,9 +390,8 @@ auto TVMQPSolver::addVirtualContactImpl(const mc_rbdyn::Contact & contact) -> st [this, contact]() { return desiredContactForce(contact); }); gui_->addElement({"Contacts", "Forces"}, mc_rtc::gui::Force( - fmt::format("{}::{}/{}::{}", r1.name(), f1.name(), r2.name(), f2.name()), - [this, contact]() { return desiredContactForce(contact); }, - [&f1]() { return f1.position(); })); + fmt::format("{}::{}/{}::{}", r1.name(), f1.name(), r2.name(), f2.name()), [this, contact]() + { return desiredContactForce(contact); }, [&f1]() { return f1.position(); })); } else { diff --git a/src/mc_solver/TasksQPSolver.cpp b/src/mc_solver/TasksQPSolver.cpp index 6876c5464a..8200f0c540 100644 --- a/src/mc_solver/TasksQPSolver.cpp +++ b/src/mc_solver/TasksQPSolver.cpp @@ -98,8 +98,7 @@ void TasksQPSolver::setContacts(ControllerToken, const std::vectoraddElement({"Contacts", "Forces"}, mc_rtc::gui::Force( fmt::format("{}::{}/{}::{}", r1, r1S, r2, r2S), - [this, &contact]() { return desiredContactForce(contact); }, - [this, &contact]() + [this, &contact]() { return desiredContactForce(contact); }, [this, &contact]() { return robots().robot(contact.r1Index()).surfacePose(contact.r1Surface()->name()); })); } } diff --git a/src/mc_tasks/AdmittanceTask.cpp b/src/mc_tasks/AdmittanceTask.cpp index 5c51397fca..dc3b23f18b 100644 --- a/src/mc_tasks/AdmittanceTask.cpp +++ b/src/mc_tasks/AdmittanceTask.cpp @@ -167,4 +167,4 @@ static auto registered = mc_tasks::MetaTaskLoader::register_load_function( t->load(solver, config); return t; }); -} +} // namespace diff --git a/src/mc_tasks/BSplineTrajectoryTask.cpp b/src/mc_tasks/BSplineTrajectoryTask.cpp index c9c7b13a17..6a25888fcd 100644 --- a/src/mc_tasks/BSplineTrajectoryTask.cpp +++ b/src/mc_tasks/BSplineTrajectoryTask.cpp @@ -159,4 +159,4 @@ static auto registered = mc_tasks::MetaTaskLoader::register_load_function( t->pause(config("paused", false)); return t; }); -} +} // namespace diff --git a/src/mc_tasks/CoMTask.cpp b/src/mc_tasks/CoMTask.cpp index 00b94a535c..db7b13e170 100644 --- a/src/mc_tasks/CoMTask.cpp +++ b/src/mc_tasks/CoMTask.cpp @@ -144,4 +144,4 @@ static auto registered = mc_tasks::MetaTaskLoader::register_load_function( t->load(solver, config); return t; }); -} +} // namespace diff --git a/src/mc_tasks/CoPTask.cpp b/src/mc_tasks/CoPTask.cpp index f600330d0c..bc4782b7a5 100644 --- a/src/mc_tasks/CoPTask.cpp +++ b/src/mc_tasks/CoPTask.cpp @@ -156,4 +156,4 @@ static auto registered = mc_tasks::MetaTaskLoader::register_load_function( t->load(solver, config); return t; }); -} +} // namespace diff --git a/src/mc_tasks/ComplianceTask.cpp b/src/mc_tasks/ComplianceTask.cpp index 2c888f0b38..77c6520017 100644 --- a/src/mc_tasks/ComplianceTask.cpp +++ b/src/mc_tasks/ComplianceTask.cpp @@ -207,4 +207,4 @@ static auto registered = mc_tasks::MetaTaskLoader::register_load_function( t->load(solver, config); return t; }); -} +} // namespace diff --git a/src/mc_tasks/DampingTask.cpp b/src/mc_tasks/DampingTask.cpp index e10240f97f..df14b97e65 100644 --- a/src/mc_tasks/DampingTask.cpp +++ b/src/mc_tasks/DampingTask.cpp @@ -80,4 +80,4 @@ static auto registered = mc_tasks::MetaTaskLoader::register_load_function( t->load(solver, config); return t; }); -} +} // namespace diff --git a/src/mc_tasks/EndEffectorTask.cpp b/src/mc_tasks/EndEffectorTask.cpp index 99dd96c85e..1b320edd1b 100644 --- a/src/mc_tasks/EndEffectorTask.cpp +++ b/src/mc_tasks/EndEffectorTask.cpp @@ -206,20 +206,19 @@ void EndEffectorTask::addToGUI(mc_rtc::gui::StateBuilder & gui) mc_rtc::gui::NumberInput( "weight", [this]() { return this->positionTask->weight(); }, [this](const double & w) { this->positionTask->weight(w); })); - gui.addElement( - {"Tasks", name_, "Gains", "Orientation"}, - mc_rtc::gui::NumberInput( - "stiffness", [this]() { return this->orientationTask->stiffness(); }, - [this](const double & s) { this->orientationTask->setGains(s, this->orientationTask->damping()); }), - mc_rtc::gui::NumberInput( - "damping", [this]() { return this->orientationTask->damping(); }, - [this](const double & d) { this->orientationTask->setGains(this->orientationTask->stiffness(), d); }), - mc_rtc::gui::NumberInput( - "stiffness & damping", [this]() { return this->orientationTask->stiffness(); }, - [this](const double & g) { this->orientationTask->stiffness(g); }), - mc_rtc::gui::NumberInput( - "weight", [this]() { return this->orientationTask->weight(); }, - [this](const double & w) { this->orientationTask->weight(w); })); + gui.addElement({"Tasks", name_, "Gains", "Orientation"}, + mc_rtc::gui::NumberInput( + "stiffness", [this]() { return this->orientationTask->stiffness(); }, [this](const double & s) + { this->orientationTask->setGains(s, this->orientationTask->damping()); }), + mc_rtc::gui::NumberInput( + "damping", [this]() { return this->orientationTask->damping(); }, [this](const double & d) + { this->orientationTask->setGains(this->orientationTask->stiffness(), d); }), + mc_rtc::gui::NumberInput( + "stiffness & damping", [this]() { return this->orientationTask->stiffness(); }, + [this](const double & g) { this->orientationTask->stiffness(g); }), + mc_rtc::gui::NumberInput( + "weight", [this]() { return this->orientationTask->weight(); }, + [this](const double & w) { this->orientationTask->weight(w); })); } void EndEffectorTask::name(const std::string & name) diff --git a/src/mc_tasks/ExactCubicTrajectoryTask.cpp b/src/mc_tasks/ExactCubicTrajectoryTask.cpp index 2e1eefe2b5..bc2ff20344 100644 --- a/src/mc_tasks/ExactCubicTrajectoryTask.cpp +++ b/src/mc_tasks/ExactCubicTrajectoryTask.cpp @@ -98,9 +98,8 @@ void ExactCubicTrajectoryTask::addToGUI(mc_rtc::gui::StateBuilder & gui) [this]() -> Eigen::Vector3d { return initialPose_.translation(); }, [this]() -> Eigen::Vector3d { return initialPose_.translation() + bspline.init_vel(); }), mc_rtc::gui::Arrow( - "Final", mc_rtc::gui::ArrowConfig(mc_rtc::gui::Color(0., 1., 1.)), - [this]() -> Eigen::Vector3d { return SplineTrajectoryBase::target().translation(); }, - [this]() -> Eigen::Vector3d + "Final", mc_rtc::gui::ArrowConfig(mc_rtc::gui::Color(0., 1., 1.)), [this]() -> Eigen::Vector3d + { return SplineTrajectoryBase::target().translation(); }, [this]() -> Eigen::Vector3d { return SplineTrajectoryBase::target().translation() + bspline.end_vel(); })); } @@ -203,4 +202,4 @@ static auto registered = mc_tasks::MetaTaskLoader::register_load_function( t->pause(config("paused", false)); return t; }); -} +} // namespace diff --git a/src/mc_tasks/FirstOrderImpedanceTask.cpp b/src/mc_tasks/FirstOrderImpedanceTask.cpp index f21aa40c70..59ec31868b 100644 --- a/src/mc_tasks/FirstOrderImpedanceTask.cpp +++ b/src/mc_tasks/FirstOrderImpedanceTask.cpp @@ -162,4 +162,4 @@ static auto registered = mc_tasks::MetaTaskLoader::register_load_function( t->load(solver, config); return t; }); -} +} // namespace diff --git a/src/mc_tasks/GazeTask.cpp b/src/mc_tasks/GazeTask.cpp index 2b5152d881..d874c1e959 100644 --- a/src/mc_tasks/GazeTask.cpp +++ b/src/mc_tasks/GazeTask.cpp @@ -201,4 +201,4 @@ static auto registered = mc_tasks::MetaTaskLoader::register_load_function( t->load(solver, config); return t; }); -} +} // namespace diff --git a/src/mc_tasks/ImpedanceTask.cpp b/src/mc_tasks/ImpedanceTask.cpp index 02c4ef20ce..1bfaff82cb 100644 --- a/src/mc_tasks/ImpedanceTask.cpp +++ b/src/mc_tasks/ImpedanceTask.cpp @@ -245,25 +245,21 @@ void ImpedanceTask::addToGUI(mc_rtc::gui::StateBuilder & gui) mc_rtc::gui::NumberInput( "cutoffPeriod", [this]() { return this->cutoffPeriod(); }, [this](double a) { return this->cutoffPeriod(a); }), - mc_rtc::gui::Checkbox( - "hold", [this]() { return hold_; }, [this]() { hold_ = !hold_; })); - gui.addElement({"Tasks", name_, "Impedance gains"}, - mc_rtc::gui::ArrayInput( - "mass", {"cx", "cy", "cz", "fx", "fy", "fz"}, - [this]() -> const sva::ImpedanceVecd & { return gains().mass().vec(); }, - [this](const Eigen::Vector6d & a) { gains().mass().vec(a); }), - mc_rtc::gui::ArrayInput( - "damper", {"cx", "cy", "cz", "fx", "fy", "fz"}, - [this]() -> const sva::ImpedanceVecd & { return gains().damper().vec(); }, - [this](const Eigen::Vector6d & a) { gains().damper().vec(a); }), - mc_rtc::gui::ArrayInput( - "spring", {"cx", "cy", "cz", "fx", "fy", "fz"}, - [this]() -> const sva::ImpedanceVecd & { return gains().spring().vec(); }, - [this](const Eigen::Vector6d & a) { gains().spring().vec(a); }), - mc_rtc::gui::ArrayInput( - "wrench", {"cx", "cy", "cz", "fx", "fy", "fz"}, - [this]() -> const sva::ImpedanceVecd & { return gains().wrench().vec(); }, - [this](const Eigen::Vector6d & a) { gains().wrench().vec(a); })); + mc_rtc::gui::Checkbox("hold", [this]() { return hold_; }, [this]() { hold_ = !hold_; })); + gui.addElement( + {"Tasks", name_, "Impedance gains"}, + mc_rtc::gui::ArrayInput( + "mass", {"cx", "cy", "cz", "fx", "fy", "fz"}, [this]() -> const sva::ImpedanceVecd & + { return gains().mass().vec(); }, [this](const Eigen::Vector6d & a) { gains().mass().vec(a); }), + mc_rtc::gui::ArrayInput( + "damper", {"cx", "cy", "cz", "fx", "fy", "fz"}, [this]() -> const sva::ImpedanceVecd & + { return gains().damper().vec(); }, [this](const Eigen::Vector6d & a) { gains().damper().vec(a); }), + mc_rtc::gui::ArrayInput( + "spring", {"cx", "cy", "cz", "fx", "fy", "fz"}, [this]() -> const sva::ImpedanceVecd & + { return gains().spring().vec(); }, [this](const Eigen::Vector6d & a) { gains().spring().vec(a); }), + mc_rtc::gui::ArrayInput( + "wrench", {"cx", "cy", "cz", "fx", "fy", "fz"}, [this]() -> const sva::ImpedanceVecd & + { return gains().wrench().vec(); }, [this](const Eigen::Vector6d & a) { gains().wrench().vec(a); })); } } // namespace force @@ -294,4 +290,4 @@ static auto registered = mc_tasks::MetaTaskLoader::register_load_function( t->load(solver, config); return t; }); -} +} // namespace diff --git a/src/mc_tasks/LookAtTask.cpp b/src/mc_tasks/LookAtTask.cpp index 99f26e4bed..9c9ba5e5f8 100644 --- a/src/mc_tasks/LookAtTask.cpp +++ b/src/mc_tasks/LookAtTask.cpp @@ -106,4 +106,4 @@ static auto registered_lookat = mc_tasks::MetaTaskLoader::register_load_function t->load(solver, config); return t; }); -} +} // namespace diff --git a/src/mc_tasks/MomentumTask.cpp b/src/mc_tasks/MomentumTask.cpp index 2053555ce1..58ef464871 100644 --- a/src/mc_tasks/MomentumTask.cpp +++ b/src/mc_tasks/MomentumTask.cpp @@ -115,11 +115,9 @@ void MomentumTask::addToGUI(mc_rtc::gui::StateBuilder & gui) { case Backend::Tasks: { - gui.addElement({"Tasks", name_}, mc_rtc::gui::ArrayLabel("momentum", {"cx", "cy", "cz", "fx", "fy", "fz"}, - [this]() -> Eigen::Vector6d { - return this->momentum().vector() - - tasks_error(this->errorT)->eval(); - })); + gui.addElement({"Tasks", name_}, mc_rtc::gui::ArrayLabel( + "momentum", {"cx", "cy", "cz", "fx", "fy", "fz"}, [this]() -> Eigen::Vector6d + { return this->momentum().vector() - tasks_error(this->errorT)->eval(); })); break; } case Backend::TVM: @@ -148,4 +146,4 @@ static auto registered = mc_tasks::MetaTaskLoader::register_load_function( t->load(solver, config); return t; }); -} +} // namespace diff --git a/src/mc_tasks/OrientationTask.cpp b/src/mc_tasks/OrientationTask.cpp index 95b7686324..ba35a85644 100644 --- a/src/mc_tasks/OrientationTask.cpp +++ b/src/mc_tasks/OrientationTask.cpp @@ -91,8 +91,7 @@ void OrientationTask::addToGUI(mc_rtc::gui::StateBuilder & gui) TrajectoryTaskGeneric::addToGUI(gui); gui.addElement({"Tasks", name_}, mc_rtc::gui::Rotation( - "ori_target", - [this]() -> sva::PTransformd + "ori_target", [this]() -> sva::PTransformd { return sva::PTransformd(this->orientation(), frame_->position().translation()); }, [this](const Eigen::Quaterniond & ori) { this->orientation(ori.toRotationMatrix()); }), mc_rtc::gui::Rotation("ori", [this]() -> sva::PTransformd { return frame_->position(); })); diff --git a/src/mc_tasks/PositionBasedVisServoTask.cpp b/src/mc_tasks/PositionBasedVisServoTask.cpp index 90c0e17d66..01eff5c8c0 100644 --- a/src/mc_tasks/PositionBasedVisServoTask.cpp +++ b/src/mc_tasks/PositionBasedVisServoTask.cpp @@ -178,4 +178,4 @@ static auto registered = mc_tasks::MetaTaskLoader::register_load_function( t->load(solver, config); return t; }); -} +} // namespace diff --git a/src/mc_tasks/PostureTask.cpp b/src/mc_tasks/PostureTask.cpp index 95a273486d..44ae87fc7a 100644 --- a/src/mc_tasks/PostureTask.cpp +++ b/src/mc_tasks/PostureTask.cpp @@ -643,4 +643,4 @@ static auto registered = mc_tasks::MetaTaskLoader::register_load_function( t->load(solver, config); return t; }); -} +} // namespace diff --git a/src/mc_tasks/RelativeEndEffectorTask.cpp b/src/mc_tasks/RelativeEndEffectorTask.cpp index 7244ee196d..a7f5ece1aa 100644 --- a/src/mc_tasks/RelativeEndEffectorTask.cpp +++ b/src/mc_tasks/RelativeEndEffectorTask.cpp @@ -20,10 +20,10 @@ RelativeEndEffectorTask::RelativeEndEffectorTask(const std::string & bodyName, double stiffness, double weight) : RelativeEndEffectorTask( - robots.robot(robotIndex).frame(bodyName), - robots.robot(robotIndex).frame(relBodyName.size() ? relBodyName : robots.robot(robotIndex).mb().body(0).name()), - stiffness, - weight) + robots.robot(robotIndex).frame(bodyName), + robots.robot(robotIndex).frame(relBodyName.size() ? relBodyName : robots.robot(robotIndex).mb().body(0).name()), + stiffness, + weight) { } diff --git a/src/mc_tasks/VectorOrientationTask.cpp b/src/mc_tasks/VectorOrientationTask.cpp index 622e346bd3..07bae1ebbe 100644 --- a/src/mc_tasks/VectorOrientationTask.cpp +++ b/src/mc_tasks/VectorOrientationTask.cpp @@ -204,4 +204,4 @@ static auto registered = mc_tasks::MetaTaskLoader::register_load_function( t->load(solver, config); return t; }); -} +} // namespace diff --git a/src/mc_tasks/lipm_stabilizer/StabilizerTask.cpp b/src/mc_tasks/lipm_stabilizer/StabilizerTask.cpp index 2f150d563b..351d41f113 100644 --- a/src/mc_tasks/lipm_stabilizer/StabilizerTask.cpp +++ b/src/mc_tasks/lipm_stabilizer/StabilizerTask.cpp @@ -1605,4 +1605,4 @@ static auto registered = mc_tasks::MetaTaskLoader::register_load_function( t->reset(); return t; }); -} +} // namespace diff --git a/src/mc_tasks/lipm_stabilizer/StabilizerTask_log_gui.cpp b/src/mc_tasks/lipm_stabilizer/StabilizerTask_log_gui.cpp index fe0f2e4779..6b5b065544 100644 --- a/src/mc_tasks/lipm_stabilizer/StabilizerTask_log_gui.cpp +++ b/src/mc_tasks/lipm_stabilizer/StabilizerTask_log_gui.cpp @@ -44,17 +44,14 @@ void StabilizerTask::addToGUI(mc_rtc::gui::StateBuilder & gui) {"Tasks", name_, "Main"}, ArrayInput( "Foot admittance", {"CoPx", "CoPy"}, - [this]() -> Eigen::Vector2d { - return {c_.copAdmittance.x(), c_.copAdmittance.y()}; - }, + [this]() -> Eigen::Vector2d { return {c_.copAdmittance.x(), c_.copAdmittance.y()}; }, [this](const Eigen::Vector2d & a) { copAdmittance(a); }), ArrayInput( "Foot force difference Damping", {"Fx", "Fy", "Fz"}, [this]() -> Eigen::Vector3d { return c_.dfDamping; }, [this](const Eigen::Vector3d & a) { dfDamping(a); }), ArrayInput( - "Foot force difference Admittance", {"Fx", "Fy", "Fz"}, - [this]() -> Eigen::Vector3d { return c_.dfAdmittance; }, - [this](const Eigen::Vector3d & a) { dfAdmittance(a); }), + "Foot force difference Admittance", {"Fx", "Fy", "Fz"}, [this]() -> Eigen::Vector3d + { return c_.dfAdmittance; }, [this](const Eigen::Vector3d & a) { dfAdmittance(a); }), ArrayInput( "DCM P gains", {"x", "y"}, [this]() -> const Eigen::Vector2d & { return c_.dcmPropGain; }, [this](const Eigen::Vector2d & gains) { dcmGains(gains, c_.dcmIntegralGain, c_.dcmDerivGain); }), @@ -70,9 +67,7 @@ void StabilizerTask::addToGUI(mc_rtc::gui::StateBuilder & gui) "ZMPd gain", [this]() { return c_.zmpdGain; }, [this](double a) { c_.zmpdGain = a; }), ArrayInput( "DCM filters", {"Integrator T [s]", "Derivator T [s]"}, - [this]() -> Eigen::Vector2d { - return {dcmIntegrator_.timeConstant(), dcmDerivator_.cutoffPeriod()}; - }, + [this]() -> Eigen::Vector2d { return {dcmIntegrator_.timeConstant(), dcmDerivator_.cutoffPeriod()}; }, [this](const Eigen::Vector2d & T) { dcmIntegratorTimeConstant(T(0)); @@ -103,9 +98,7 @@ void StabilizerTask::addToGUI(mc_rtc::gui::StateBuilder & gui) ArrayInput( "Foot CoP lambda", {"CoPx", "CoPy", "f"}, - [this]() -> Eigen::Vector3d { - return {c_.copFzLambda.x(), c_.copFzLambda.y(), c_.copFzLambda.z()}; - }, + [this]() -> Eigen::Vector3d { return {c_.copFzLambda.x(), c_.copFzLambda.y(), c_.copFzLambda.z()}; }, [this](const Eigen::Vector3d & a) { c_.copFzLambda = a; }), NumberInput( "Admittance Delay", [this]() { return c_.delayCoP; }, [this](double d) { c_.delayCoP = d; }), @@ -119,9 +112,7 @@ void StabilizerTask::addToGUI(mc_rtc::gui::StateBuilder & gui) }), ArrayInput( "Vertical drift compensation", {"frequency", "stiffness"}, - [this]() -> Eigen::Vector2d { - return {c_.vdcFrequency, c_.vdcStiffness}; - }, + [this]() -> Eigen::Vector2d { return {c_.vdcFrequency, c_.vdcStiffness}; }, [this](const Eigen::Vector2d & v) { vdcFrequency(v(0)); @@ -246,8 +237,7 @@ void StabilizerTask::addToGUI(mc_rtc::gui::StateBuilder & gui) "zmp_ref", [this]() { return zmpTarget_.x(); }, Color::Cyan), plot::Y( "zmp_mes", [this]() { return measuredZMP_.x(); }, Color::Blue, Style::Dashed), - plot::Y( - "zmp_stabi", [this]() { return distribZMP_.x(); }, Color::Blue)); + plot::Y("zmp_stabi", [this]() { return distribZMP_.x(); }, Color::Blue)); }), Button("Stop DCM-ZMP (x)", [&gui]() { gui.removePlot("DCM-ZMP Tracking (x)"); })); @@ -271,8 +261,7 @@ void StabilizerTask::addToGUI(mc_rtc::gui::StateBuilder & gui) "zmp_ref", [this]() { return zmpTarget_.y(); }, Color::Cyan), plot::Y( "zmp_mes", [this]() { return measuredZMP_.y(); }, Color::Blue, Style::Dashed), - plot::Y( - "zmp_stabi", [this]() { return distribZMP_.y(); }, Color::Blue)); + plot::Y("zmp_stabi", [this]() { return distribZMP_.y(); }, Color::Blue)); }), Button("Stop DCM-ZMP (y)", [&gui]() { gui.removePlot("DCM-ZMP Tracking (y)"); })); @@ -283,8 +272,7 @@ void StabilizerTask::addToGUI(mc_rtc::gui::StateBuilder & gui) gui.addPlot("CoM Tracking (x)", plot::X("t", [this]() { return t_; }), plot::Y( "com_ref", [this]() { return comTarget_.x(); }, Color::Red), - plot::Y( - "com_mes", [this]() { return measuredCoM_.x(); }, Color::Magenta)); + plot::Y("com_mes", [this]() { return measuredCoM_.x(); }, Color::Magenta)); }), Button("Stop CoM (x)", [&gui]() { gui.removePlot("CoM Tracking (x)"); })); gui.addElement({"Tasks", name_, "Debug"}, ElementsStacking::Horizontal, @@ -294,8 +282,7 @@ void StabilizerTask::addToGUI(mc_rtc::gui::StateBuilder & gui) gui.addPlot("CoM Tracking (y)", plot::X("t", [this]() { return t_; }), plot::Y( "com_ref", [this]() { return comTarget_.y(); }, Color::Red), - plot::Y( - "com_mes", [this]() { return measuredCoM_.y(); }, Color::Magenta)); + plot::Y("com_mes", [this]() { return measuredCoM_.y(); }, Color::Magenta)); }), Button("Stop CoM (y)", [&gui]() { gui.removePlot("CoM Tracking (y)"); })); @@ -308,8 +295,7 @@ void StabilizerTask::addToGUI(mc_rtc::gui::StateBuilder & gui) "x", [this]() { return dcmIntegrator_.eval().x(); }, Color::Red), plot::Y( "y", [this]() { return dcmIntegrator_.eval().y(); }, Color::Green), - plot::Y( - "z", [this]() { return dcmIntegrator_.eval().z(); }, Color::Blue)); + plot::Y("z", [this]() { return dcmIntegrator_.eval().z(); }, Color::Blue)); }), Button("Stop DCM Integrator", [&gui]() { gui.removePlot("DCM Integrator"); })); gui.addElement({"Tasks", name_, "Debug"}, ElementsStacking::Horizontal, @@ -321,8 +307,7 @@ void StabilizerTask::addToGUI(mc_rtc::gui::StateBuilder & gui) "x", [this]() { return dcmDerivator_.eval().x(); }, Color::Red), plot::Y( "y", [this]() { return dcmDerivator_.eval().y(); }, Color::Green), - plot::Y( - "z", [this]() { return dcmDerivator_.eval().z(); }, Color::Blue)); + plot::Y("z", [this]() { return dcmDerivator_.eval().z(); }, Color::Blue)); }), Button("Stop DCM Derivator", [&gui]() { gui.removePlot("DCM Derivator"); })); @@ -388,15 +373,14 @@ void StabilizerTask::addToGUI(mc_rtc::gui::StateBuilder & gui) for(const auto footTask : footTasks) { auto footT = footTask.second; - gui.addElement({"Tasks", name_, "Markers", "Foot wrenches"}, - Point3D("Stabilizer_" + footT->surface() + "CoP", PointConfig(Color::Magenta, 0.01), - [footT]() { return footT->targetCoPW(); }), - Force( - "Measured_" + footT->surface() + "CoPForce", copForceConfig, - [footT, this]() { - return robot().indirectSurfaceForceSensor(footT->surface()).worldWrenchWithoutGravity(robot()); - }, - [footT]() { return sva::PTransformd(footT->measuredCoPW()); })); + gui.addElement( + {"Tasks", name_, "Markers", "Foot wrenches"}, + Point3D("Stabilizer_" + footT->surface() + "CoP", PointConfig(Color::Magenta, 0.01), + [footT]() { return footT->targetCoPW(); }), + Force( + "Measured_" + footT->surface() + "CoPForce", copForceConfig, [footT, this]() + { return robot().indirectSurfaceForceSensor(footT->surface()).worldWrenchWithoutGravity(robot()); }, + [footT]() { return sva::PTransformd(footT->measuredCoPW()); })); } gui.addElement({"Tasks", name_, "Markers", "Contacts"}, diff --git a/src/mc_trajectory/BSpline.cpp b/src/mc_trajectory/BSpline.cpp index f911093064..05d0a3bdd2 100644 --- a/src/mc_trajectory/BSpline.cpp +++ b/src/mc_trajectory/BSpline.cpp @@ -73,8 +73,8 @@ void BSpline::addToGUI(mc_rtc::gui::StateBuilder & gui, const std::vector const std::vector & { return samples_; })); + gui.addElement(category, mc_rtc::gui::Trajectory("Trajectory", [this]() -> const std::vector & + { return samples_; })); // Interactive control points std::vector waypointCategory = category; diff --git a/src/mc_trajectory/ExactCubic.cpp b/src/mc_trajectory/ExactCubic.cpp index d35e108615..bafe45dfb0 100644 --- a/src/mc_trajectory/ExactCubic.cpp +++ b/src/mc_trajectory/ExactCubic.cpp @@ -134,8 +134,8 @@ void ExactCubic::addToGUI(mc_rtc::gui::StateBuilder & gui, const std::vector const Eigen::Vector3d & { return target(); }, [this](const Eigen::Vector3d & pos) { target(pos); })); - gui.addElement(category, mc_rtc::gui::Trajectory( - "Trajectory", [this]() -> const std::vector & { return samples_; })); + gui.addElement(category, mc_rtc::gui::Trajectory("Trajectory", [this]() -> const std::vector & + { return samples_; })); // Interactive control points (target is handled independently) std::vector waypointCategory = category; @@ -144,9 +144,8 @@ void ExactCubic::addToGUI(mc_rtc::gui::StateBuilder & gui, const std::vector const Eigen::Vector3d & { return waypoint(i).second; }, - [this, i](const Eigen::Vector3d & pos) { waypoint(i, pos); }), + "Waypoint " + std::to_string(i), [this, i]() -> const Eigen::Vector3d & + { return waypoint(i).second; }, [this, i](const Eigen::Vector3d & pos) { waypoint(i, pos); }), mc_rtc::gui::NumberInput( "Time " + std::to_string(i), [this, i]() { return waypointTime(i); }, [this, i](double t) { waypoint(i, t); })); diff --git a/tests/controllers/TestCoMInBoxController.cpp b/tests/controllers/TestCoMInBoxController.cpp index b7686649c8..ed7899b408 100644 --- a/tests/controllers/TestCoMInBoxController.cpp +++ b/tests/controllers/TestCoMInBoxController.cpp @@ -24,9 +24,8 @@ namespace mc_control /** Build a cube as a set of planes from a given origin and size */ static std::vector makeCube(const Eigen::Vector3d & origin, double size) { - auto getPlane = [&](const Eigen::Vector3d & normal, const Eigen::Vector3d & point) { - return mc_rbdyn::Plane{normal, -normal.dot(origin + point)}; - }; + auto getPlane = [&](const Eigen::Vector3d & normal, const Eigen::Vector3d & point) + { return mc_rbdyn::Plane{normal, -normal.dot(origin + point)}; }; return {getPlane(Eigen::Vector3d{1, 0, 0}, Eigen::Vector3d{-size, 0, 0}), getPlane(Eigen::Vector3d{-1, 0, 0}, Eigen::Vector3d{size, 0, 0}), getPlane(Eigen::Vector3d{0, 1, 0}, Eigen::Vector3d{0, -size, 0}), diff --git a/tests/gui_SampleServer.cpp b/tests/gui_SampleServer.cpp index 98cab614f7..6780cfbf7c 100644 --- a/tests/gui_SampleServer.cpp +++ b/tests/gui_SampleServer.cpp @@ -321,8 +321,8 @@ SampleServer::SampleServer() : xythetaz_(4) builder.addElement({"Buttons"}, mc_rtc::gui::ElementsStacking::Horizontal, mc_rtc::gui::Button("Foo", []() { mc_rtc::log::info("Foo pushed"); }), mc_rtc::gui::Button("Bar", []() { mc_rtc::log::info("Bar pushed"); })); - builder.addElement({"Checkbox"}, mc_rtc::gui::Checkbox( - "Checkbox", [this]() { return check_; }, [this]() { check_ = !check_; })); + builder.addElement({"Checkbox"}, + mc_rtc::gui::Checkbox("Checkbox", [this]() { return check_; }, [this]() { check_ = !check_; })); builder.addElement({"Checkbox", "Simple syntax"}, mc_rtc::gui::Checkbox("Checkbox", check_)); builder.addElement({"Inputs"}, mc_rtc::gui::StringInput( @@ -394,17 +394,15 @@ SampleServer::SampleServer() : xythetaz_(4) builder.addElement({"Inputs", "Generic"}, mc_rtc::gui::Input("StringInput", string_), mc_rtc::gui::Input("IntegerInput", int_), mc_rtc::gui::Input("NumberInput", d_), mc_rtc::gui::Input("ArrayInput", v_), mc_rtc::gui::Input("ArrayInput w/ auto labels", v3_)); - builder.addElement({"Schema"}, mc_rtc::gui::Schema("Add metatask", "MetaTask", - [](const mc_rtc::Configuration & c) { - std::cout << "Got schema request:\n" - << c.dump(true) << std::endl; - })); + builder.addElement({"Schema"}, + mc_rtc::gui::Schema("Add metatask", "MetaTask", [](const mc_rtc::Configuration & c) + { std::cout << "Got schema request:\n" + << c.dump(true) << std::endl; })); builder.addElement({"Forms", "Static"}, mc_rtc::gui::Form( - "Add contact", - [](const mc_rtc::Configuration & data) { - std::cout << "Got data" << std::endl << data.dump(true) << std::endl; - }, + "Add contact", [](const mc_rtc::Configuration & data) + { std::cout << "Got data" << std::endl + << data.dump(true) << std::endl; }, mc_rtc::gui::FormCheckbox("Enabled", false, true), mc_rtc::gui::FormIntegerInput("INT", false, 42), mc_rtc::gui::FormNumberInput("NUMBER", false, 0.42), @@ -450,10 +448,8 @@ SampleServer::SampleServer() : xythetaz_(4) mc_rtc::gui::Transform( "Interactive Transform", [this]() { return interactive_; }, [this](const sva::PTransformd & p) { interactive_ = p; }), - mc_rtc::gui::XYTheta("XYTheta ReadOnly", - [this]() -> std::array { - return {xytheta_.x(), xytheta_.y(), xytheta_.z(), 0.1}; - }), + mc_rtc::gui::XYTheta("XYTheta ReadOnly", [this]() -> std::array + { return {xytheta_.x(), xytheta_.y(), xytheta_.z(), 0.1}; }), mc_rtc::gui::XYTheta( "XYTheta", [this]() { return xytheta_; }, [this](const Eigen::VectorXd & vec) { xytheta_ = vec.head<3>(); }), mc_rtc::gui::XYTheta( @@ -647,44 +643,29 @@ SampleServer::SampleServer() : xythetaz_(4) {"GUI Markers", "Trajectories"}, mc_rtc::gui::Trajectory("Vector3d", [this]() { return trajectory_; }), mc_rtc::gui::Trajectory("PTransformd", {mc_rtc::gui::Color::Green}, [this]() { return poseTrajectory_; }), mc_rtc::gui::Trajectory("Live 3D", {mc_rtc::gui::Color::Magenta}, - [this]() { - return Eigen::Vector3d{cos(t_), sin(t_), 1.0}; - }), - mc_rtc::gui::Trajectory("Live transform", {mc_rtc::gui::Color::Black}, - [this]() { - return lookAt({cos(t_), -1, sin(t_)}, {0, 0, 0}, Eigen::Vector3d::UnitZ()); - })); + [this]() { return Eigen::Vector3d{cos(t_), sin(t_), 1.0}; }), + mc_rtc::gui::Trajectory("Live transform", {mc_rtc::gui::Color::Black}, [this]() + { return lookAt({cos(t_), -1, sin(t_)}, {0, 0, 0}, Eigen::Vector3d::UnitZ()); })); mc_rtc::gui::ArrowConfig arrow_config({1., 0., 0.}); arrow_config.start_point_scale = 0.02; arrow_config.end_point_scale = 0.02; builder.addElement({"GUI Markers", "Arrows"}, mc_rtc::gui::Arrow( - "ArrowRO", arrow_config, - []() { - return Eigen::Vector3d{2, 2, 0}; - }, - []() { - return Eigen::Vector3d{2.5, 2.5, 0.5}; - }), + "ArrowRO", arrow_config, []() { return Eigen::Vector3d{2, 2, 0}; }, + []() { return Eigen::Vector3d{2.5, 2.5, 0.5}; }), mc_rtc::gui::Arrow( "Arrow", arrow_config, [this]() { return arrow_start_; }, [this](const Eigen::Vector3d & start) { arrow_start_ = start; }, [this]() { return arrow_end_; }, [this](const Eigen::Vector3d & end) { arrow_end_ = end; }), mc_rtc::gui::Force( "ForceRO", mc_rtc::gui::ForceConfig(mc_rtc::gui::Color(1., 0., 0.)), - []() { - return sva::ForceVecd(Eigen::Vector3d{0., 0., 0.}, Eigen::Vector3d{10., 0., 100.}); - }, - []() { - return sva::PTransformd{Eigen::Vector3d{2, 2, 0}}; - }), + []() { return sva::ForceVecd(Eigen::Vector3d{0., 0., 0.}, Eigen::Vector3d{10., 0., 100.}); }, + []() { return sva::PTransformd{Eigen::Vector3d{2, 2, 0}}; }), mc_rtc::gui::Force( "Force", mc_rtc::gui::ForceConfig(mc_rtc::gui::Color(0., 1., 0.)), [this]() { return force_; }, [this](const sva::ForceVecd & force) { force_ = force; }, - []() { - return sva::PTransformd{Eigen::Vector3d{2, 2, 0}}; - })); + []() { return sva::PTransformd{Eigen::Vector3d{2, 2, 0}}; })); using Color = mc_rtc::gui::Color; using Range = mc_rtc::gui::plot::Range; using Style = mc_rtc::gui::plot::Style; @@ -694,8 +675,7 @@ SampleServer::SampleServer() : xythetaz_(4) builder.addPlot(name, mc_rtc::gui::plot::X("t", [this]() { return t_; }), mc_rtc::gui::plot::Y( "sin(t)", [this]() { return std::sin(t_); }, Color::Red), - mc_rtc::gui::plot::Y( - "cos(t)", [this]() { return std::cos(t_); }, Color::Blue)); + mc_rtc::gui::plot::Y("cos(t)", [this]() { return std::cos(t_); }, Color::Blue)); }; add_demo_plot("sin(t)/cos(t)", sin_cos_plot); auto discontinuous_plot = [this](const std::string & name) @@ -729,12 +709,12 @@ SampleServer::SampleServer() : xythetaz_(4) add_demo_plot("Demo style", demo_style_plot); auto fix_axis_plot = [this](const std::string & name) { - builder.addPlot(name, mc_rtc::gui::plot::X("t", [this]() { return t_; }), {"Y1", {0, 1}}, // Fix both min and max - {"Y2", {-Range::inf, 0}}, // Only fix max - mc_rtc::gui::plot::Y( - "sin(t)", [this]() { return std::sin(t_); }, Color::Red), - mc_rtc::gui::plot::Y( - "cos(t)", [this]() { return std::cos(t_); }, Color::Blue, Style::Solid, Side::Right)); + builder.addPlot( + name, mc_rtc::gui::plot::X("t", [this]() { return t_; }), {"Y1", {0, 1}}, // Fix both min and max + {"Y2", {-Range::inf, 0}}, // Only fix max + mc_rtc::gui::plot::Y( + "sin(t)", [this]() { return std::sin(t_); }, Color::Red), + mc_rtc::gui::plot::Y("cos(t)", [this]() { return std::cos(t_); }, Color::Blue, Style::Solid, Side::Right)); }; add_demo_plot("Fix axis", fix_axis_plot); using PolygonDescription = mc_rtc::gui::plot::PolygonDescription; @@ -786,8 +766,7 @@ SampleServer::SampleServer() : xythetaz_(4) builder.addXYPlot(name); n_dynamic_xy_plot = 0; // Add an Y-plot, this should return false - bool added = builder.addPlotData(name, mc_rtc::gui::plot::Y( - "label", []() { return 0.0; }, Color::Blue)); + bool added = builder.addPlotData(name, mc_rtc::gui::plot::Y("label", []() { return 0.0; }, Color::Blue)); assert(!added); (void)added; }; @@ -826,8 +805,7 @@ SampleServer::SampleServer() : xythetaz_(4) } else { - builder.addPlotData(name, mc_rtc::gui::plot::Y( - label, [data]() { return data; }, Color::Blue)); + builder.addPlotData(name, mc_rtc::gui::plot::Y(label, [data]() { return data; }, Color::Blue)); } n_plots += 1; }; @@ -878,9 +856,8 @@ void SampleServer::switch_visual(const std::string & choice) visualChoice_ = choice; builder.removeCategory({"Visual"}); builder.addElement({"Visual"}, mc_rtc::gui::ComboInput( - "Choice", {"sphere", "box", "cylinder", "mesh"}, - [this]() -> const std::string & { return visualChoice_; }, - [this](const std::string & c) { switch_visual(c); })); + "Choice", {"sphere", "box", "cylinder", "mesh"}, [this]() -> const std::string & + { return visualChoice_; }, [this](const std::string & c) { switch_visual(c); })); if(choice == "sphere") { visual_ = mc_rtc::makeVisualSphere(sphereRadius_, {1, 0, 0, 0.2}); @@ -959,34 +936,33 @@ void SampleServer::switch_visual(const std::string & choice) builder.addElement({"Visual"}, mc_rtc::gui::Visual( choice, [this]() -> const rbd::parsers::Visual & { return visual_; }, [this]() -> const sva::PTransformd & { return visualPos_; })); - builder.addElement( - {"Visual", "Ellipsoid"}, - mc_rtc::gui::Ellipsoid( - "Fixed size/Fixed color", Eigen::Vector3d(0.25, 0.5, 1.0), - []() { return sva::PTransformd(Eigen::Vector3d(-4, 0, 1)); }, mc_rtc::gui::Color::Blue), - mc_rtc::gui::Ellipsoid( - "Fixed size/Varying color", Eigen::Vector3d(0.25, 0.5, 1.0), - []() { return sva::PTransformd(Eigen::Vector3d(-4, 1, 1)); }, - [this]() - { - auto color = mc_rtc::gui::Color::Yellow; - color.a = (1 + cos(t_)) / 2; - return color; - }), - mc_rtc::gui::Ellipsoid( - "Varying size/Fixed color", - [this]() -> Eigen::Vector3d { return Eigen::Vector3d(0.25, 0.5, 1.0) * (1 + (1 + cos(t_)) / 2); }, - []() { return sva::PTransformd(Eigen::Vector3d(-4, 2, 1)); }), - mc_rtc::gui::Ellipsoid( - "Varying size/Varying color", - [this]() -> Eigen::Vector3d { return Eigen::Vector3d(0.25, 0.5, 1.0) * (1 + (1 + sin(t_)) / 2); }, - []() { return sva::PTransformd(Eigen::Vector3d(-4, 3, 1)); }, - [this]() - { - auto color = mc_rtc::gui::Color::Green; - color.a = (1 + sin(t_)) / 2; - return color; - })); + builder.addElement({"Visual", "Ellipsoid"}, + mc_rtc::gui::Ellipsoid( + "Fixed size/Fixed color", Eigen::Vector3d(0.25, 0.5, 1.0), + []() { return sva::PTransformd(Eigen::Vector3d(-4, 0, 1)); }, mc_rtc::gui::Color::Blue), + mc_rtc::gui::Ellipsoid( + "Fixed size/Varying color", Eigen::Vector3d(0.25, 0.5, 1.0), + []() { return sva::PTransformd(Eigen::Vector3d(-4, 1, 1)); }, + [this]() + { + auto color = mc_rtc::gui::Color::Yellow; + color.a = (1 + cos(t_)) / 2; + return color; + }), + mc_rtc::gui::Ellipsoid( + "Varying size/Fixed color", [this]() -> Eigen::Vector3d + { return Eigen::Vector3d(0.25, 0.5, 1.0) * (1 + (1 + cos(t_)) / 2); }, + []() { return sva::PTransformd(Eigen::Vector3d(-4, 2, 1)); }), + mc_rtc::gui::Ellipsoid( + "Varying size/Varying color", [this]() -> Eigen::Vector3d + { return Eigen::Vector3d(0.25, 0.5, 1.0) * (1 + (1 + sin(t_)) / 2); }, + []() { return sva::PTransformd(Eigen::Vector3d(-4, 3, 1)); }, + [this]() + { + auto color = mc_rtc::gui::Color::Green; + color.a = (1 + sin(t_)) / 2; + return color; + })); builder.addElement({"Visual", "Cylinder"}, mc_rtc::gui::Cylinder( "Fixed size/Fixed color", {0.125, 1.0}, @@ -1022,34 +998,33 @@ void SampleServer::switch_visual(const std::string & choice) color.a = (1 + sin(t_)) / 2; return color; })); - builder.addElement( - {"Visual", "Box"}, - mc_rtc::gui::Box( - "Fixed size/Fixed color", Eigen::Vector3d(0.25, 0.5, 1.0), - []() { return sva::PTransformd(Eigen::Vector3d(-2, 0, 1)); }, mc_rtc::gui::Color::Blue), - mc_rtc::gui::Box( - "Fixed size/Varying color", Eigen::Vector3d(0.25, 0.5, 1.0), - []() { return sva::PTransformd(Eigen::Vector3d(-2, 1, 1)); }, - [this]() - { - auto color = mc_rtc::gui::Color::Yellow; - color.a = (1 + cos(t_)) / 2; - return color; - }), - mc_rtc::gui::Box( - "Varying size/Fixed color", - [this]() -> Eigen::Vector3d { return Eigen::Vector3d(0.25, 0.5, 1.0) * (1 + (1 + cos(t_)) / 2); }, - []() { return sva::PTransformd(Eigen::Vector3d(-2, 2, 1)); }), - mc_rtc::gui::Box( - "Varying size/Varying color", - [this]() -> Eigen::Vector3d { return Eigen::Vector3d(0.25, 0.5, 1.0) * (1 + (1 + sin(t_)) / 2); }, - []() { return sva::PTransformd(Eigen::Vector3d(-2, 3, 1)); }, - [this]() - { - auto color = mc_rtc::gui::Color::Green; - color.a = (1 + sin(t_)) / 2; - return color; - })); + builder.addElement({"Visual", "Box"}, + mc_rtc::gui::Box( + "Fixed size/Fixed color", Eigen::Vector3d(0.25, 0.5, 1.0), + []() { return sva::PTransformd(Eigen::Vector3d(-2, 0, 1)); }, mc_rtc::gui::Color::Blue), + mc_rtc::gui::Box( + "Fixed size/Varying color", Eigen::Vector3d(0.25, 0.5, 1.0), + []() { return sva::PTransformd(Eigen::Vector3d(-2, 1, 1)); }, + [this]() + { + auto color = mc_rtc::gui::Color::Yellow; + color.a = (1 + cos(t_)) / 2; + return color; + }), + mc_rtc::gui::Box( + "Varying size/Fixed color", [this]() -> Eigen::Vector3d + { return Eigen::Vector3d(0.25, 0.5, 1.0) * (1 + (1 + cos(t_)) / 2); }, + []() { return sva::PTransformd(Eigen::Vector3d(-2, 2, 1)); }), + mc_rtc::gui::Box( + "Varying size/Varying color", [this]() -> Eigen::Vector3d + { return Eigen::Vector3d(0.25, 0.5, 1.0) * (1 + (1 + sin(t_)) / 2); }, + []() { return sva::PTransformd(Eigen::Vector3d(-2, 3, 1)); }, + [this]() + { + auto color = mc_rtc::gui::Color::Green; + color.a = (1 + sin(t_)) / 2; + return color; + })); builder.addElement({"Visual", "Sphere"}, mc_rtc::gui::Sphere( "Fixed radius/Fixed color", 0.25, []() { return sva::PTransformd(Eigen::Vector3d(-1, 0, 1)); }, diff --git a/tests/testLogger.cpp b/tests/testLogger.cpp index 5963f56e84..d5e21e4d1b 100644 --- a/tests/testLogger.cpp +++ b/tests/testLogger.cpp @@ -70,7 +70,7 @@ struct LogData public: #define DEFINE_GETTER(MEMBER) \ - auto get_##MEMBER() const -> const decltype(this->MEMBER) & { return this->MEMBER; } + auto get_##MEMBER() const->const decltype(this->MEMBER) & { return this->MEMBER; } DEFINE_GETTER(b) DEFINE_GETTER(d) DEFINE_GETTER(s) @@ -86,9 +86,9 @@ struct LogData DEFINE_GETTER(v) #undef DEFINE_GETTER -#define DEFINE_GET_AS_REF(MEMBER) \ - auto get_##MEMBER##_as_ref() const -> Eigen::RefMEMBER)> { return this->MEMBER; } \ - auto get_##MEMBER##_as_cref() const -> Eigen::RefMEMBER)> { return this->MEMBER; } +#define DEFINE_GET_AS_REF(MEMBER) \ + auto get_##MEMBER##_as_ref() const->Eigen::RefMEMBER)> { return this->MEMBER; } \ + auto get_##MEMBER##_as_cref() const->Eigen::RefMEMBER)> { return this->MEMBER; } DEFINE_GET_AS_REF(v2d) DEFINE_GET_AS_REF(v3d) DEFINE_GET_AS_REF(v6d) diff --git a/tests/test_mc_rbdyn.cpp b/tests/test_mc_rbdyn.cpp index 00d97b48eb..c0ca6427e3 100644 --- a/tests/test_mc_rbdyn.cpp +++ b/tests/test_mc_rbdyn.cpp @@ -101,13 +101,11 @@ BOOST_AUTO_TEST_CASE(TestRobotPosWVelWAccW) auto checkVelocity = [](const sva::MotionVecd & actual, const sva::MotionVecd & refVal) { - BOOST_CHECK_MESSAGE(actual.vector().isApprox(refVal.vector()), "Error in Robot::velW" - << "\nExpected:" - << "\nangular:" << refVal.angular().transpose() - << "\nlinear :" << refVal.linear().transpose() - << "\nGot:" - << "\nangular:" << actual.angular().transpose() - << "\nlinear :" << actual.linear().transpose()); + BOOST_CHECK_MESSAGE(actual.vector().isApprox(refVal.vector()), + "Error in Robot::velW" << "\nExpected:" << "\nangular:" << refVal.angular().transpose() + << "\nlinear :" << refVal.linear().transpose() + << "\nGot:" << "\nangular:" << actual.angular().transpose() + << "\nlinear :" << actual.linear().transpose()); }; for(int i = 0; i < 100; ++i) @@ -153,9 +151,9 @@ BOOST_AUTO_TEST_CASE(TestRobotZMPSimple) auto zmpIdeal = X_0_ls.translation(); auto zmpComputed = robot.zmp(sensorNames, Eigen::Vector3d::Zero(), {0., 0., 1.}); - BOOST_CHECK_MESSAGE(zmpComputed.isApprox(zmpIdeal, 1e-10), "Error in Robot::zmp computation with leftFootRatio=" - << "\nExpected: " << zmpIdeal.transpose() - << "\nGot: " << zmpComputed.transpose()); + BOOST_CHECK_MESSAGE(zmpComputed.isApprox(zmpIdeal, 1e-10), + "Error in Robot::zmp computation with leftFootRatio=" << "\nExpected: " << zmpIdeal.transpose() + << "\nGot: " << zmpComputed.transpose()); } { @@ -169,9 +167,9 @@ BOOST_AUTO_TEST_CASE(TestRobotZMPSimple) auto zmpIdeal = X_0_rs.translation(); auto zmpComputed = robot.zmp(sensorNames, Eigen::Vector3d::Zero(), {0., 0., 1.}); - BOOST_CHECK_MESSAGE(zmpComputed.isApprox(zmpIdeal, 1e-10), "Error in Robot::zmp computation with leftFootRatio=" - << "\nExpected: " << zmpIdeal.transpose() - << "\nGot: " << zmpComputed.transpose()); + BOOST_CHECK_MESSAGE(zmpComputed.isApprox(zmpIdeal, 1e-10), + "Error in Robot::zmp computation with leftFootRatio=" << "\nExpected: " << zmpIdeal.transpose() + << "\nGot: " << zmpComputed.transpose()); } { // checks that zmp throws if used with null force diff --git a/utils/RobotVisualizer.cpp b/utils/RobotVisualizer.cpp index f88d7e11a5..860e79d049 100644 --- a/utils/RobotVisualizer.cpp +++ b/utils/RobotVisualizer.cpp @@ -76,8 +76,7 @@ void RobotVisualizer::setupRobotSelector() { if(builder.hasElement({}, "Switch robot")) { builder.removeElement({}, "Switch robot"); } builder.addElement({}, mc_rtc::gui::Form( - "Switch robot", - [this](const mc_rtc::Configuration & data) + "Switch robot", [this](const mc_rtc::Configuration & data) { this->loadRobot({data("Robot").operator std::string()}); }, mc_rtc::gui::FormComboInput("Robot", true, available_robots, false, selected_robot))); }