diff --git a/CMakeLists.txt b/CMakeLists.txt index c69ba488..34efe808 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -136,7 +136,8 @@ set(CartesianInterface_SRC src/problem/impl/Cartesian.cpp src/problem/impl/Com.cpp src/problem/impl/Limits.cpp - # src/problem/Gaze.cpp + src/problem/impl/OmniWheels4X.cpp + src/problem/impl/Gaze.cpp src/problem/impl/Postural.cpp src/problem/impl/Subtask.cpp src/problem/impl/Interaction.cpp @@ -162,6 +163,8 @@ set(CartesianInterfaceSOT_SRC src/opensot/task_adapters/OpenSotJointLimits.cpp src/opensot/task_adapters/OpenSotVelocityLimits.cpp src/opensot/task_adapters/OpenSotConstraintFromTask.cpp + src/opensot/task_adapters/OpenSotOmniWheels4X.cpp + src/opensot/task_adapters/OpenSotGaze.cpp src/opensot/OpenSotImpl.cpp ) diff --git a/include/cartesian_interface/problem/Gaze.h b/include/cartesian_interface/problem/Gaze.h index 9eaec334..762aa5a6 100644 --- a/include/cartesian_interface/problem/Gaze.h +++ b/include/cartesian_interface/problem/Gaze.h @@ -8,29 +8,11 @@ namespace XBot { namespace Cartesian { /** * @brief Description of a Gaze task */ - struct GazeTask : CartesianTask { + struct GazeTask : virtual CartesianTask { typedef std::shared_ptr Ptr; typedef std::shared_ptr ConstPtr; - - GazeTask() = default; - GazeTask(std::string base_link = "world"); - - static TaskDescription::Ptr yaml_parse_gaze(YAML::Node node, ModelInterface::ConstPtr model); }; - - /** - * @brief Make a Gaze task and return a shared pointer - */ - GazeTask::Ptr MakeGaze(std::string base_link = "world"); - - /** - * @brief Dynamic cast a generic task to a GazeTask - * - * @return A null pointer if the cast is unsuccessful (i.e. task is not a GazeTask) - */ - GazeTask::Ptr GetAsGaze(TaskDescription::Ptr task); - } } diff --git a/include/cartesian_interface/sdk/opensot/OpenSotGaze.h b/include/cartesian_interface/sdk/opensot/OpenSotGaze.h new file mode 100644 index 00000000..956f8af1 --- /dev/null +++ b/include/cartesian_interface/sdk/opensot/OpenSotGaze.h @@ -0,0 +1,51 @@ +#ifndef OPENSOTGAZEADAPTER_H +#define OPENSOTGAZEADAPTER_H + +#include + +#include +#include "OpenSotTask.h" + +#include + +using GazeSoT = OpenSoT::tasks::velocity::Gaze; + +namespace XBot { namespace Cartesian { + +class OpenSotGazeAdapter : + public OpenSotTaskAdapter, + public virtual CartesianTaskObserver +{ + +public: + + OpenSotGazeAdapter(TaskDescription::Ptr task, + Context::ConstPtr context); + + virtual TaskPtr constructTask() override; + + virtual bool initialize(const OpenSoT::OptvarHelper& vars) override; + + virtual void update(double time, double period) override; + + bool onBaseLinkChanged() override; + + bool onControlModeChanged() override; + + virtual ~OpenSotGazeAdapter() override = default; + +protected: + + GazeSoT::Ptr _opensot_cart; + +private: + + GazeTask::Ptr _ci_cart; +}; + + + + +} } + +#endif // OPENSOTTASKADAPTER_H diff --git a/include/cartesian_interface/sdk/opensot/OpenSotOmniWheels4X.h b/include/cartesian_interface/sdk/opensot/OpenSotOmniWheels4X.h new file mode 100644 index 00000000..0b9dc755 --- /dev/null +++ b/include/cartesian_interface/sdk/opensot/OpenSotOmniWheels4X.h @@ -0,0 +1,43 @@ +#ifndef OPENSOTOMNIWHEELS4X_H +#define OPENSOTOMNIWHEELS4X_H + +#include + +#include +#include "OpenSotTask.h" + +#include + +using OmniWheels4XSoT = OpenSoT::constraints::velocity::OmniWheels4X; + +namespace XBot { namespace Cartesian { + +class OpenSotOmniWheels4XAdapter : + public OpenSotConstraintAdapter +{ + +public: + + OpenSotOmniWheels4XAdapter(ConstraintDescription::Ptr constr, + Context::ConstPtr context); + + virtual ConstraintPtr constructConstraint() override; + + virtual bool initialize(const OpenSoT::OptvarHelper& vars) override; + + virtual ~OpenSotOmniWheels4XAdapter() override = default; + +protected: + +private: + + OmniWheels4XSoT::Ptr _opensot_omniwheels4x; + OmniWheels4X::Ptr _ci_omniwheels4x; +}; + + + + +} } + +#endif // OPENSOTOMNIWHEELS4X_H diff --git a/include/cartesian_interface/sdk/problem/Gaze.h b/include/cartesian_interface/sdk/problem/Gaze.h new file mode 100644 index 00000000..ad1ee796 --- /dev/null +++ b/include/cartesian_interface/sdk/problem/Gaze.h @@ -0,0 +1,30 @@ +#ifndef __XBOT_CARTESIAN_PROBLEM_GAZE_IMPL_H__ +#define __XBOT_CARTESIAN_PROBLEM_GAZE_IMPL_H__ + +#include +#include + +namespace XBot { namespace Cartesian { + + /** + * @brief Description of a center of mass task + */ + struct GazeTaskImpl : public CartesianTaskImpl, + public virtual GazeTask + { + + CARTESIO_DECLARE_SMART_PTR(GazeTaskImpl) + + + GazeTaskImpl(Context::ConstPtr context, + std::string name, + std::string distal_link, + std::string base_link = "world"); + + GazeTaskImpl(YAML::Node node, Context::ConstPtr context); + }; + +} } + + +#endif diff --git a/include/cartesian_interface/sdk/problem/OmniWheels4X.h b/include/cartesian_interface/sdk/problem/OmniWheels4X.h new file mode 100644 index 00000000..e6d1939e --- /dev/null +++ b/include/cartesian_interface/sdk/problem/OmniWheels4X.h @@ -0,0 +1,36 @@ +#ifndef __XBOT_CARTESIAN_PROBLEM_OMNIWHEELS4X_H__ +#define __XBOT_CARTESIAN_PROBLEM_OMNIWHEELS4X_H__ + +#include +#include + +namespace XBot { namespace Cartesian { + + class OmniWheels4X : public virtual ConstraintDescription, public virtual TaskDescriptionImpl + { + + public: + + CARTESIO_DECLARE_SMART_PTR(OmniWheels4X) + + OmniWheels4X(YAML::Node yaml, Context::ConstPtr context); + + double getl1() { return _l1; } + double getl2() { return _l2; } + double getr() { return _r; } + const std::string& get_base_link() const { return _base_link; } + const std::vector get_joint_wheels_name() const { return _joint_wheels_name; } + + private: + + double _l1, _l2, _r; + std::vector _joint_wheels_name; + std::string _base_link; + }; + + + +} } + + +#endif diff --git a/src/opensot/task_adapters/OpenSotGaze.cpp b/src/opensot/task_adapters/OpenSotGaze.cpp new file mode 100644 index 00000000..90691e37 --- /dev/null +++ b/src/opensot/task_adapters/OpenSotGaze.cpp @@ -0,0 +1,76 @@ +#include "opensot/OpenSotGaze.h" + +using namespace XBot::Cartesian; + +OpenSotGazeAdapter::OpenSotGazeAdapter(TaskDescription::Ptr task, + Context::ConstPtr context): + OpenSotTaskAdapter(task, context) +{ + _ci_cart = std::dynamic_pointer_cast(task); + + if(!_ci_cart) throw std::runtime_error("Provided task description " + "does not have expected type 'GazeTask'"); + +} + +TaskPtr OpenSotGazeAdapter::constructTask() +{ + Eigen::VectorXd q; + _model->getJointPosition(q); + + _opensot_cart = SotUtils::make_shared(_ci_cart->getName(), + q, + const_cast(*_model), + _ci_cart->getBaseLink(), + _ci_cart->getDistalLink()); + + return _opensot_cart; +} + +bool OpenSotGazeAdapter::initialize(const OpenSoT::OptvarHelper& vars) +{ + if(vars.getAllVariables().size() > 0) + { + throw BadVariables("[OpenSotGazeAdapter] requires default variables definition"); + } + + bool ret = OpenSotTaskAdapter::initialize(vars); + if(!ret) return false; + + + /* Register observer */ + auto this_shared_ptr = std::dynamic_pointer_cast(shared_from_this()); + _ci_cart->registerObserver(this_shared_ptr); + + return true; +} + +void OpenSotGazeAdapter::update(double time, double period) +{ + // note: this will update lambda + OpenSotTaskAdapter::update(time, period); + + // we implement velocity control by forcing lambda = 0.0 + auto ctrl = _ci_cart->getControlMode(); + + if(ctrl == ControlType::Velocity) + { + _opensot_cart->setLambda(0.0); + } + + /* Update reference */ + Eigen::Affine3d Tref; + Eigen::Vector6d vref; + _ci_cart->getPoseReference(Tref, &vref); + _opensot_cart->setGaze(Tref); +} + +bool OpenSotGazeAdapter::onBaseLinkChanged() +{ + return _opensot_cart->setBaseLink(_ci_cart->getBaseLink()); +} + +bool OpenSotGazeAdapter::onControlModeChanged() +{ + return true; +} diff --git a/src/opensot/task_adapters/OpenSotOmniWheels4X.cpp b/src/opensot/task_adapters/OpenSotOmniWheels4X.cpp new file mode 100644 index 00000000..3aef9c87 --- /dev/null +++ b/src/opensot/task_adapters/OpenSotOmniWheels4X.cpp @@ -0,0 +1,40 @@ +#include "opensot/OpenSotOmniWheels4X.h" + +using namespace XBot::Cartesian; + +OpenSotOmniWheels4XAdapter::OpenSotOmniWheels4XAdapter(ConstraintDescription::Ptr constr, Context::ConstPtr context): + OpenSotConstraintAdapter(constr, context) +{ + _ci_omniwheels4x = std::dynamic_pointer_cast(constr); + + if(!_ci_omniwheels4x) throw std::runtime_error("Provided task description " + "does not have expected type 'OmniWheels4X'"); +} + +ConstraintPtr OpenSotOmniWheels4XAdapter::constructConstraint() +{ + Eigen::VectorXd q; + _model->getJointPosition(q); + + _opensot_omniwheels4x = SotUtils::make_shared(_ci_omniwheels4x->getl1(), + _ci_omniwheels4x->getl2(), + _ci_omniwheels4x->getr(), + _ci_omniwheels4x->get_joint_wheels_name(), + _ci_omniwheels4x->get_base_link(), + q, const_cast(*_model)); + + return _opensot_omniwheels4x; +} + +bool OpenSotOmniWheels4XAdapter::initialize(const OpenSoT::OptvarHelper &vars) +{ + if(vars.getAllVariables().size() > 0) + { + throw BadVariables("[OpenSotOmniWheels4XAdapter] requires default variables definition"); + } + + bool ret = OpenSotConstraintAdapter::initialize(vars); + if(!ret) return false; + + return true; +} diff --git a/src/opensot/task_adapters/OpenSotTask.cpp b/src/opensot/task_adapters/OpenSotTask.cpp index cdeeb297..b179dd26 100644 --- a/src/opensot/task_adapters/OpenSotTask.cpp +++ b/src/opensot/task_adapters/OpenSotTask.cpp @@ -11,6 +11,8 @@ #include "opensot/OpenSotPostural.h" #include "opensot/OpenSotCom.h" #include "opensot/OpenSotSubtask.h" +#include "opensot/OpenSotOmniWheels4X.h" +#include "opensot/OpenSotGaze.h" #include "fmt/format.h" @@ -169,6 +171,10 @@ OpenSotTaskAdapter::Ptr OpenSotTaskAdapter::MakeInstance(TaskDescription::Ptr ta { task_adapter = new OpenSotComAdapter(task, context); } + else if(task->getType() == "Gaze") + { + task_adapter = new OpenSotGazeAdapter(task, context); + } else if(task->getType() == "Subtask") /* Otherwise, construct supported tasks */ { task_adapter = new OpenSotSubtaskAdapter(task, context); @@ -287,6 +293,10 @@ OpenSotConstraintAdapter::Ptr OpenSotConstraintAdapter::MakeInstance(ConstraintD { constr_adapter = new OpenSotVelocityLimitsAdapter(constr, context); } + else if(constr->getType() == "OmniWheels4X") + { + constr_adapter = new OpenSotOmniWheels4XAdapter(constr, context); + } else { auto str = fmt::format("Unable to construct OpenSotConstraintAdapter instance for task '{}': " diff --git a/src/problem/impl/Cartesian.cpp b/src/problem/impl/Cartesian.cpp index 69d2de32..3f490bf4 100644 --- a/src/problem/impl/Cartesian.cpp +++ b/src/problem/impl/Cartesian.cpp @@ -68,6 +68,11 @@ int get_size(YAML::Node task_node) return 3; } + if(task_node["type"].as() == "Gaze") + { + return 2; + } + return 6; } diff --git a/src/problem/impl/Gaze.cpp b/src/problem/impl/Gaze.cpp index e69de29b..1773ae11 100644 --- a/src/problem/impl/Gaze.cpp +++ b/src/problem/impl/Gaze.cpp @@ -0,0 +1,19 @@ +#include "problem/Gaze.h" + +using namespace XBot::Cartesian; + +GazeTaskImpl::GazeTaskImpl(Context::ConstPtr context, + std::string name, + std::string distal_link, + std::string base_link): + CartesianTaskImpl(context, name, distal_link, base_link) +{ + +} + +GazeTaskImpl::GazeTaskImpl(YAML::Node node, + Context::ConstPtr context): + CartesianTaskImpl(node, context) +{ + +} diff --git a/src/problem/impl/OmniWheels4X.cpp b/src/problem/impl/OmniWheels4X.cpp new file mode 100644 index 00000000..6c391998 --- /dev/null +++ b/src/problem/impl/OmniWheels4X.cpp @@ -0,0 +1,47 @@ +#include "problem/OmniWheels4X.h" + +using namespace XBot::Cartesian; + +OmniWheels4X::OmniWheels4X(YAML::Node yaml, Context::ConstPtr context): + TaskDescriptionImpl(yaml, + context, + "OmniWheels4X", + context->model()->getJointNum()), + _base_link("base_link") +{ + if(yaml["l1"]) + { + _l1 = yaml["l1"].as(); + } + else + throw std::invalid_argument("Missing mandatory l1 argument!"); + + if(yaml["l2"]) + { + _l2 = yaml["l2"].as(); + } + else + throw std::invalid_argument("Missing mandatory l2 argument!"); + + if(yaml["wheel_radius"]) + { + _r = yaml["wheel_radius"].as(); + } + else + throw std::invalid_argument("Missing mandatory wheel_radius argument!"); + + if(yaml["base_link"]) + { + _base_link = yaml["base_link"].as(); + } + + if(yaml["joint_wheels_name"]) + { + for(auto joint_name : yaml["joint_wheels_name"]) + _joint_wheels_name.push_back(joint_name.as()); + } + else + throw std::invalid_argument("Missing mandatory joint_wheels_name argument!"); + + +} diff --git a/src/problem/impl/TaskFactory.cpp b/src/problem/impl/TaskFactory.cpp index b5dc12b0..8948727d 100644 --- a/src/problem/impl/TaskFactory.cpp +++ b/src/problem/impl/TaskFactory.cpp @@ -5,11 +5,14 @@ #include "problem/Cartesian.h" #include "problem/Interaction.h" #include "problem/Com.h" +#include "problem/Gaze.h" #include "problem/Postural.h" #include "problem/Limits.h" #include "problem/Subtask.h" +#include "problem/OmniWheels4X.h" + #include "fmt/format.h" namespace XBot { namespace Cartesian { @@ -125,10 +128,14 @@ std::shared_ptr MakeTaskDescription(YAML::Node prob_desc, { task_desc = std::make_shared(task_node, context); } - // else if(task_type == "Gaze") - // { - // task_desc = GazeTask::yaml_parse_gaze(task_node, model); - // } + else if(task_type == "OmniWheels4X") + { + task_desc = std::make_shared(task_node, context); + } + else if(task_type == "Gaze") + { + task_desc = std::make_shared(task_node, context); + } // else if(task_type == "MinJointVel") // { // task_desc = MinJointVelTask::yaml_parse_minjointvel(task_node, model);