Gaze

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
  )
 

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<GazeTask> Ptr;
  typedef std::shared_ptr<const GazeTask> 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);
-
 
 
 } }

include/cartesian_interface/sdk/opensot/OpenSotGaze.h

@@ -0,0 +1,51 @@
+#ifndef OPENSOTGAZEADAPTER_H
+#define OPENSOTGAZEADAPTER_H
+
+#include <boost/make_shared.hpp>
+
+#include <cartesian_interface/problem/Gaze.h>
+#include "OpenSotTask.h"
+
+#include <OpenSoT/tasks/velocity/Gaze.h>
+
+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

include/cartesian_interface/sdk/opensot/OpenSotOmniWheels4X.h

@@ -0,0 +1,43 @@
+#ifndef OPENSOTOMNIWHEELS4X_H
+#define OPENSOTOMNIWHEELS4X_H
+
+#include <boost/make_shared.hpp>
+
+#include <cartesian_interface/sdk/problem/OmniWheels4X.h>
+#include "OpenSotTask.h"
+
+#include <OpenSoT/constraints/velocity/OmniWheels4X.h>
+
+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

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 <cartesian_interface/problem/Gaze.h>
+#include <cartesian_interface/sdk/problem/Cartesian.h>
+
+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

include/cartesian_interface/sdk/problem/OmniWheels4X.h

@@ -0,0 +1,36 @@
+#ifndef __XBOT_CARTESIAN_PROBLEM_OMNIWHEELS4X_H__
+#define __XBOT_CARTESIAN_PROBLEM_OMNIWHEELS4X_H__
+
+#include <cartesian_interface/sdk/problem/Constraint.h>
+#include <cartesian_interface/Macro.h>
+
+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<std::string> get_joint_wheels_name() const { return _joint_wheels_name; }
+
+ private:
+
+ double _l1, _l2, _r;
+ std::vector<std::string> _joint_wheels_name;
+ std::string _base_link;
+ };
+
+
+
+} }
+
+
+#endif

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<GazeTask>(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<GazeSoT>(_ci_cart->getName(),
+ q,
+ const_cast<ModelInterface&>(*_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<OpenSotGazeAdapter>(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;
+}

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<OmniWheels4X>(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<OmniWheels4XSoT>(_ci_omniwheels4x->getl1(),
+ _ci_omniwheels4x->getl2(),
+ _ci_omniwheels4x->getr(),
+ _ci_omniwheels4x->get_joint_wheels_name(),
+ _ci_omniwheels4x->get_base_link(),
+ q, const_cast<ModelInterface&>(*_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;
+}

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 '{}': "

src/problem/impl/Cartesian.cpp

@@ -68,6 +68,11 @@ int get_size(YAML::Node task_node)
  return 3;
  }
 
+ if(task_node["type"].as<std::string>() == "Gaze")
+ {
+ return 2;
+ }
+
  return 6;
 }
 

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)
+{
+
+}