Commit 6f8471e4 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

[c++] Add CoP task with python bindings.

parent 2605f9c0
...@@ -107,6 +107,7 @@ SET(${PROJECT_NAME}_TASKS_HEADERS ...@@ -107,6 +107,7 @@ SET(${PROJECT_NAME}_TASKS_HEADERS
include/tsid/tasks/task-com-equality.hpp include/tsid/tasks/task-com-equality.hpp
include/tsid/tasks/task-se3-equality.hpp include/tsid/tasks/task-se3-equality.hpp
include/tsid/tasks/task-contact-force-equality.hpp include/tsid/tasks/task-contact-force-equality.hpp
include/tsid/tasks/task-cop-equality.hpp
include/tsid/tasks/task-actuation-equality.hpp include/tsid/tasks/task-actuation-equality.hpp
include/tsid/tasks/task-actuation-bounds.hpp include/tsid/tasks/task-actuation-bounds.hpp
include/tsid/tasks/task-joint-bounds.hpp include/tsid/tasks/task-joint-bounds.hpp
...@@ -150,6 +151,7 @@ SET(${PROJECT_NAME}_ROBOTS_HEADERS ...@@ -150,6 +151,7 @@ SET(${PROJECT_NAME}_ROBOTS_HEADERS
) )
SET(${PROJECT_NAME}_FORMULATIONS_HEADERS SET(${PROJECT_NAME}_FORMULATIONS_HEADERS
include/tsid/formulations/contact-level.hpp
include/tsid/formulations/inverse-dynamics-formulation-base.hpp include/tsid/formulations/inverse-dynamics-formulation-base.hpp
include/tsid/formulations/inverse-dynamics-formulation-acc-force.hpp include/tsid/formulations/inverse-dynamics-formulation-acc-force.hpp
) )
...@@ -198,6 +200,7 @@ SET(${PROJECT_NAME}_TASKS_SOURCES ...@@ -198,6 +200,7 @@ SET(${PROJECT_NAME}_TASKS_SOURCES
src/tasks/task-com-equality.cpp src/tasks/task-com-equality.cpp
src/tasks/task-contact-force-equality.cpp src/tasks/task-contact-force-equality.cpp
src/tasks/task-contact-force.cpp src/tasks/task-contact-force.cpp
src/tasks/task-cop-equality.cpp
src/tasks/task-joint-bounds.cpp src/tasks/task-joint-bounds.cpp
src/tasks/task-joint-posture.cpp src/tasks/task-joint-posture.cpp
src/tasks/task-joint-posVelAcc-bounds.cpp src/tasks/task-joint-posVelAcc-bounds.cpp
...@@ -232,6 +235,7 @@ SET(${PROJECT_NAME}_ROBOTS_SOURCES ...@@ -232,6 +235,7 @@ SET(${PROJECT_NAME}_ROBOTS_SOURCES
) )
SET(${PROJECT_NAME}_FORMULATIONS_SOURCES SET(${PROJECT_NAME}_FORMULATIONS_SOURCES
src/formulations/contact-level.cpp
src/formulations/inverse-dynamics-formulation-base.cpp src/formulations/inverse-dynamics-formulation-base.cpp
src/formulations/inverse-dynamics-formulation-acc-force.cpp src/formulations/inverse-dynamics-formulation-acc-force.cpp
) )
......
//
// Copyright (c) 2021 University of Trento
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//
#include "tsid/bindings/python/tasks/task-cop-equality.hpp"
#include "tsid/bindings/python/tasks/expose-tasks.hpp"
namespace tsid
{
namespace python
{
void exposeTaskCopEquality()
{
TaskCOPEqualityPythonVisitor<tsid::tasks::TaskCopEquality>::expose("TaskCopEquality");
}
}
}
...@@ -31,6 +31,7 @@ ...@@ -31,6 +31,7 @@
#include "tsid/tasks/task-joint-posture.hpp" #include "tsid/tasks/task-joint-posture.hpp"
#include "tsid/tasks/task-se3-equality.hpp" #include "tsid/tasks/task-se3-equality.hpp"
#include "tsid/tasks/task-com-equality.hpp" #include "tsid/tasks/task-com-equality.hpp"
#include "tsid/tasks/task-cop-equality.hpp"
#include "tsid/tasks/task-actuation-bounds.hpp" #include "tsid/tasks/task-actuation-bounds.hpp"
#include "tsid/tasks/task-joint-bounds.hpp" #include "tsid/tasks/task-joint-bounds.hpp"
#include "tsid/tasks/task-angular-momentum-equality.hpp" #include "tsid/tasks/task-angular-momentum-equality.hpp"
...@@ -62,6 +63,7 @@ namespace tsid ...@@ -62,6 +63,7 @@ namespace tsid
.def("addMotionTask", &InvDynPythonVisitor::addMotionTask_Joint, bp::args("task", "weight", "priorityLevel", "transition duration")) .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_Joint, bp::args("task", "weight", "priorityLevel", "transition duration"))
.def("addMotionTask", &InvDynPythonVisitor::addMotionTask_JointBounds, bp::args("task", "weight", "priorityLevel", "transition duration")) .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_JointBounds, bp::args("task", "weight", "priorityLevel", "transition duration"))
.def("addMotionTask", &InvDynPythonVisitor::addMotionTask_AM, bp::args("task", "weight", "priorityLevel", "transition duration")) .def("addMotionTask", &InvDynPythonVisitor::addMotionTask_AM, bp::args("task", "weight", "priorityLevel", "transition duration"))
.def("addForceTask", &InvDynPythonVisitor::addForceTask_COP, bp::args("task", "weight", "priorityLevel", "transition duration"))
.def("addActuationTask", &InvDynPythonVisitor::addActuationTask_Bounds, bp::args("task", "weight", "priorityLevel", "transition duration")) .def("addActuationTask", &InvDynPythonVisitor::addActuationTask_Bounds, bp::args("task", "weight", "priorityLevel", "transition duration"))
.def("updateTaskWeight", &InvDynPythonVisitor::updateTaskWeight, bp::args("task_name", "weight")) .def("updateTaskWeight", &InvDynPythonVisitor::updateTaskWeight, bp::args("task_name", "weight"))
.def("updateRigidContactWeights", &InvDynPythonVisitor::updateRigidContactWeights, bp::args("contact_name", "force_regularization_weight")) .def("updateRigidContactWeights", &InvDynPythonVisitor::updateRigidContactWeights, bp::args("contact_name", "force_regularization_weight"))
...@@ -102,6 +104,9 @@ namespace tsid ...@@ -102,6 +104,9 @@ namespace tsid
static bool addMotionTask_AM(T & self, tasks::TaskAMEquality & task, double weight, unsigned int priorityLevel, double transition_duration){ static bool addMotionTask_AM(T & self, tasks::TaskAMEquality & task, double weight, unsigned int priorityLevel, double transition_duration){
return self.addMotionTask(task, weight, priorityLevel, transition_duration); return self.addMotionTask(task, weight, priorityLevel, transition_duration);
} }
static bool addForceTask_COP(T & self, tasks::TaskCopEquality & task, double weight, unsigned int priorityLevel, double transition_duration){
return self.addForceTask(task, weight, priorityLevel, transition_duration);
}
static bool addActuationTask_Bounds(T & self, tasks::TaskActuationBounds & task, double weight, unsigned int priorityLevel, double transition_duration){ static bool addActuationTask_Bounds(T & self, tasks::TaskActuationBounds & task, double weight, unsigned int priorityLevel, double transition_duration){
return self.addActuationTask(task, weight, priorityLevel, transition_duration); return self.addActuationTask(task, weight, priorityLevel, transition_duration);
} }
......
...@@ -19,6 +19,7 @@ ...@@ -19,6 +19,7 @@
#define __tsid_python_expose_tasks_hpp__ #define __tsid_python_expose_tasks_hpp__
#include "tsid/bindings/python/tasks/task-com-equality.hpp" #include "tsid/bindings/python/tasks/task-com-equality.hpp"
#include "tsid/bindings/python/tasks/task-cop-equality.hpp"
#include "tsid/bindings/python/tasks/task-se3-equality.hpp" #include "tsid/bindings/python/tasks/task-se3-equality.hpp"
#include "tsid/bindings/python/tasks/task-joint-posture.hpp" #include "tsid/bindings/python/tasks/task-joint-posture.hpp"
#include "tsid/bindings/python/tasks/task-actuation-bounds.hpp" #include "tsid/bindings/python/tasks/task-actuation-bounds.hpp"
...@@ -31,6 +32,7 @@ namespace tsid ...@@ -31,6 +32,7 @@ namespace tsid
namespace python namespace python
{ {
void exposeTaskComEquality(); void exposeTaskComEquality();
void exposeTaskCopEquality();
void exposeTaskSE3Equality(); void exposeTaskSE3Equality();
void exposeTaskJointPosture(); void exposeTaskJointPosture();
void exposeTaskActuationBounds(); void exposeTaskActuationBounds();
...@@ -40,6 +42,7 @@ namespace tsid ...@@ -40,6 +42,7 @@ namespace tsid
inline void exposeTasks() inline void exposeTasks()
{ {
exposeTaskComEquality(); exposeTaskComEquality();
exposeTaskCopEquality();
exposeTaskSE3Equality(); exposeTaskSE3Equality();
exposeTaskJointPosture(); exposeTaskJointPosture();
exposeTaskActuationBounds(); exposeTaskActuationBounds();
......
//
// Copyright (c) 2021 University of Trento
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//
#ifndef __tsid_python_task_cop_hpp__
#define __tsid_python_task_cop_hpp__
#include <pinocchio/fwd.hpp>
#include <boost/python.hpp>
#include <string>
#include <eigenpy/eigenpy.hpp>
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
#include "tsid/tasks/task-cop-equality.hpp"
#include "tsid/robots/robot-wrapper.hpp"
#include "tsid/trajectories/trajectory-base.hpp"
#include "tsid/math/constraint-equality.hpp"
#include "tsid/math/constraint-base.hpp"
namespace tsid
{
namespace python
{
namespace bp = boost::python;
template<typename TaskCOP>
struct TaskCOPEqualityPythonVisitor
: public boost::python::def_visitor< TaskCOPEqualityPythonVisitor<TaskCOP> >
{
template<class PyClass>
void visit(PyClass& cl) const
{
cl
.def(bp::init<std::string, robots::RobotWrapper &> ((bp::arg("name"), bp::arg("robot")), "Default Constructor"))
.add_property("dim", &TaskCOP::dim, "return dimension size")
.def("setReference", &TaskCOPEqualityPythonVisitor::setReference, bp::arg("ref"))
.def("compute", &TaskCOPEqualityPythonVisitor::compute, bp::args("t", "q", "v", "data"))
.def("getConstraint", &TaskCOPEqualityPythonVisitor::getConstraint)
.add_property("name", &TaskCOPEqualityPythonVisitor::name)
;
}
static std::string name(TaskCOP & self){
std::string name = self.name();
return name;
}
static math::ConstraintEquality compute(TaskCOP & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, pinocchio::Data & data){
self.compute(t, q, v, data);
math::ConstraintEquality cons(self.getConstraint().name(), self.getConstraint().matrix(), self.getConstraint().vector());
return cons;
}
static math::ConstraintEquality getConstraint(const TaskCOP & self){
math::ConstraintEquality cons(self.getConstraint().name(), self.getConstraint().matrix(), self.getConstraint().vector());
return cons;
}
static void setReference(TaskCOP & self, const Eigen::Vector3d & ref){
self.setReference(ref);
}
static void expose(const std::string & class_name)
{
std::string doc = "TaskCOPEqualityPythonVisitor info.";
bp::class_<TaskCOP>(class_name.c_str(),
doc.c_str(),
bp::no_init)
.def(TaskCOPEqualityPythonVisitor<TaskCOP>());
// bp::register_ptr_to_python< boost::shared_ptr<math::ConstraintBase> >();
}
};
}
}
#endif // ifndef __tsid_python_task_cop_hpp__
...@@ -39,7 +39,6 @@ namespace tsid ...@@ -39,7 +39,6 @@ namespace tsid
typedef math::Vector6 Vector6; typedef math::Vector6 Vector6;
typedef math::Vector3 Vector3; typedef math::Vector3 Vector3;
typedef math::Vector Vector; typedef math::Vector Vector;
typedef tasks::TaskMotion TaskMotion;
typedef tasks::TaskSE3Equality TaskSE3Equality; typedef tasks::TaskSE3Equality TaskSE3Equality;
typedef math::ConstraintInequality ConstraintInequality; typedef math::ConstraintInequality ConstraintInequality;
typedef math::ConstraintEquality ConstraintEquality; typedef math::ConstraintEquality ConstraintEquality;
...@@ -87,7 +86,7 @@ namespace tsid ...@@ -87,7 +86,7 @@ namespace tsid
ConstRefVector v, ConstRefVector v,
const Data & data); const Data & data);
const TaskMotion & getMotionTask() const; const TaskSE3Equality & getMotionTask() const;
const ConstraintBase & getMotionConstraint() const; const ConstraintBase & getMotionConstraint() const;
const ConstraintInequality & getForceConstraint() const; const ConstraintInequality & getForceConstraint() const;
const ConstraintEquality & getForceRegularizationTask() const; const ConstraintEquality & getForceRegularizationTask() const;
...@@ -95,6 +94,7 @@ namespace tsid ...@@ -95,6 +94,7 @@ namespace tsid
double getNormalForce(ConstRefVector f) const; double getNormalForce(ConstRefVector f) const;
double getMinNormalForce() const; double getMinNormalForce() const;
double getMaxNormalForce() const; double getMaxNormalForce() const;
const Matrix3x & getContactPoints() const;
const Vector & Kp() const; const Vector & Kp() const;
const Vector & Kd() const; const Vector & Kd() const;
......
...@@ -20,7 +20,7 @@ ...@@ -20,7 +20,7 @@
#include "tsid/math/fwd.hpp" #include "tsid/math/fwd.hpp"
#include "tsid/robots/fwd.hpp" #include "tsid/robots/fwd.hpp"
#include "tsid/tasks/task-motion.hpp" #include "tsid/tasks/task-se3-equality.hpp"
namespace tsid namespace tsid
...@@ -41,7 +41,8 @@ namespace tsid ...@@ -41,7 +41,8 @@ namespace tsid
typedef math::ConstraintEquality ConstraintEquality; typedef math::ConstraintEquality ConstraintEquality;
typedef math::ConstRefVector ConstRefVector; typedef math::ConstRefVector ConstRefVector;
typedef math::Matrix Matrix; typedef math::Matrix Matrix;
typedef tasks::TaskMotion TaskMotion; typedef math::Matrix3x Matrix3x;
typedef tasks::TaskSE3Equality TaskSE3Equality;
typedef pinocchio::Data Data; typedef pinocchio::Data Data;
typedef robots::RobotWrapper RobotWrapper; typedef robots::RobotWrapper RobotWrapper;
...@@ -75,7 +76,7 @@ namespace tsid ...@@ -75,7 +76,7 @@ namespace tsid
ConstRefVector v, ConstRefVector v,
const Data & data) = 0; const Data & data) = 0;
virtual const TaskMotion & getMotionTask() const = 0; virtual const TaskSE3Equality & getMotionTask() const = 0;
virtual const ConstraintBase & getMotionConstraint() const = 0; virtual const ConstraintBase & getMotionConstraint() const = 0;
virtual const ConstraintInequality & getForceConstraint() const = 0; virtual const ConstraintInequality & getForceConstraint() const = 0;
virtual const ConstraintEquality & getForceRegularizationTask() const = 0; virtual const ConstraintEquality & getForceRegularizationTask() const = 0;
...@@ -85,6 +86,7 @@ namespace tsid ...@@ -85,6 +86,7 @@ namespace tsid
virtual bool setMinNormalForce(const double minNormalForce) = 0; virtual bool setMinNormalForce(const double minNormalForce) = 0;
virtual bool setMaxNormalForce(const double maxNormalForce) = 0; virtual bool setMaxNormalForce(const double maxNormalForce) = 0;
virtual double getNormalForce(ConstRefVector f) const = 0; virtual double getNormalForce(ConstRefVector f) const = 0;
virtual const Matrix3x & getContactPoints() const = 0;
protected: protected:
std::string m_name; std::string m_name;
......
...@@ -38,7 +38,6 @@ namespace tsid ...@@ -38,7 +38,6 @@ namespace tsid
typedef math::Vector6 Vector6; typedef math::Vector6 Vector6;
typedef math::Vector3 Vector3; typedef math::Vector3 Vector3;
typedef math::Vector Vector; typedef math::Vector Vector;
typedef tasks::TaskMotion TaskMotion;
typedef tasks::TaskSE3Equality TaskSE3Equality; typedef tasks::TaskSE3Equality TaskSE3Equality;
typedef math::ConstraintInequality ConstraintInequality; typedef math::ConstraintInequality ConstraintInequality;
typedef math::ConstraintEquality ConstraintEquality; typedef math::ConstraintEquality ConstraintEquality;
...@@ -75,18 +74,19 @@ namespace tsid ...@@ -75,18 +74,19 @@ namespace tsid
ConstRefVector v, ConstRefVector v,
const Data & data); const Data & data);
const TaskMotion & getMotionTask() const; const TaskSE3Equality & getMotionTask() const;
const ConstraintBase & getMotionConstraint() const; const ConstraintBase & getMotionConstraint() const;
const ConstraintInequality & getForceConstraint() const; const ConstraintInequality & getForceConstraint() const;
const ConstraintEquality & getForceRegularizationTask() const; const ConstraintEquality & getForceRegularizationTask() const;
double getMotionTaskWeight() const; double getMotionTaskWeight() const;
const Matrix3x & getContactPoints() const;
double getNormalForce(ConstRefVector f) const; double getNormalForce(ConstRefVector f) const;
double getMinNormalForce() const; double getMinNormalForce() const;
double getMaxNormalForce() const; double getMaxNormalForce() const;
const Vector & Kp() const; const Vector & Kp(); // cannot be const because it set a member variable inside
const Vector & Kd() const; const Vector & Kd(); // cannot be const because it set a member variable inside
void Kp(ConstRefVector Kp); void Kp(ConstRefVector Kp);
void Kd(ConstRefVector Kp); void Kd(ConstRefVector Kp);
...@@ -123,6 +123,8 @@ namespace tsid ...@@ -123,6 +123,8 @@ namespace tsid
Vector3 m_contactNormal; Vector3 m_contactNormal;
Vector3 m_fRef; Vector3 m_fRef;
Vector3 m_weightForceRegTask; Vector3 m_weightForceRegTask;
Matrix3x m_contactPoints;
Vector m_Kp3, m_Kd3; // gain vectors to be returned by reference
double m_mu; double m_mu;
double m_fMin; double m_fMin;
double m_fMax; double m_fMax;
......
//
// Copyright (c) 2021 University of Trento
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//
#ifndef __tsid_contact_level_hpp__
#define __tsid_contact_level_hpp__
#include "tsid/math/fwd.hpp"
#include "tsid/contacts/contact-base.hpp"
namespace tsid
{
/** Data structure collecting information regarding a single contact.
* In particular, this structure contains the index of the force corresponding
* to this contact in the force vector used as decision variable in the QP.
* Moreover it contains all the default constraints associated to a contact for representing
* the motion constraints (contact points do not move), the friction cone constraints
* and the force regularization cost.
*/
struct ContactLevel
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
contacts::ContactBase & contact;
std::shared_ptr<math::ConstraintBase> motionConstraint;
std::shared_ptr<math::ConstraintInequality> forceConstraint;
std::shared_ptr<math::ConstraintEquality> forceRegTask;
unsigned int index; /// index of 1st element of associated force variable in the force vector
ContactLevel(contacts::ContactBase & contact);
};
}
#endif // ifndef __tsid_contact_level_hpp__
// //
// Copyright (c) 2017 CNRS, NYU, MPI Tübingen // Copyright (c) 2017 CNRS, NYU, MPI Tübingen, UNITN
// //
// This file is part of tsid // This file is part of tsid
// tsid is free software: you can redistribute it // tsid is free software: you can redistribute it
...@@ -19,38 +19,13 @@ ...@@ -19,38 +19,13 @@
#define __invdyn_inverse_dynamics_formulation_acc_force_hpp__ #define __invdyn_inverse_dynamics_formulation_acc_force_hpp__
#include "tsid/formulations/inverse-dynamics-formulation-base.hpp" #include "tsid/formulations/inverse-dynamics-formulation-base.hpp"
#include "tsid/formulations/contact-level.hpp"
#include "tsid/math/constraint-equality.hpp" #include "tsid/math/constraint-equality.hpp"
#include <vector> #include <vector>
namespace tsid namespace tsid
{ {
class TaskLevel
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
tasks::TaskBase & task;
std::shared_ptr<math::ConstraintBase> constraint;
unsigned int priority;
TaskLevel(tasks::TaskBase & task,
unsigned int priority);
};
class ContactLevel
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
contacts::ContactBase & contact;
std::shared_ptr<math::ConstraintBase> motionConstraint;
std::shared_ptr<math::ConstraintInequality> forceConstraint;
std::shared_ptr<math::ConstraintEquality> forceRegTask;
unsigned int index; /// index of 1st element of associated force variable in the force vector
ContactLevel(contacts::ContactBase & contact);
};
class ContactTransitionInfo class ContactTransitionInfo
{ {
...@@ -138,7 +113,8 @@ namespace tsid ...@@ -138,7 +113,8 @@ namespace tsid
public: public:
void addTask(std::shared_ptr<TaskLevel> task, template<class TaskLevelPointer>
void addTask(TaskLevelPointer task,
double weight, double weight,
unsigned int priorityLevel); unsigned int priorityLevel);
...@@ -150,10 +126,10 @@ namespace tsid ...@@ -150,10 +126,10 @@ namespace tsid
Data m_data; Data m_data;
HQPData m_hqpData; HQPData m_hqpData;
std::vector<std::shared_ptr<TaskLevel> > m_taskMotions; std::vector<std::shared_ptr<TaskLevel> > m_taskMotions;
std::vector<std::shared_ptr<TaskLevel> > m_taskContactForces; std::vector<std::shared_ptr<TaskLevelForce> > m_taskContactForces;
std::vector<std::shared_ptr<TaskLevel> > m_taskActuations; std::vector<std::shared_ptr<TaskLevel> > m_taskActuations;
std::vector<std::shared_ptr<ContactLevel> > m_contacts; std::vector<std::shared_ptr<ContactLevel> > m_contacts;
double m_t; /// time double m_t; /// time
unsigned int m_k; /// number of contact-force variables unsigned int m_k; /// number of contact-force variables
unsigned int m_v; /// number of acceleration variables unsigned int m_v; /// number of acceleration variables
......
...@@ -32,6 +32,30 @@ ...@@ -32,6 +32,30 @@
namespace tsid namespace tsid
{ {
struct TaskLevel
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
tasks::TaskBase & task;
std::shared_ptr<math::ConstraintBase> constraint;
unsigned int priority;
TaskLevel(tasks::TaskBase & task,
unsigned int priority);
};
struct TaskLevelForce
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
tasks::TaskContactForce & task;
std::shared_ptr<math::ConstraintBase> constraint;
unsigned int priority;
TaskLevelForce(tasks::TaskContactForce & task,
unsigned int priority);
};
/// ///
/// \brief Wrapper for a robot based on pinocchio /// \brief Wrapper for a robot based on pinocchio
/// ///
......
...@@ -19,6 +19,9 @@ ...@@ -19,6 +19,9 @@
#define __invdyn_task_contact_force_hpp__ #define __invdyn_task_contact_force_hpp__
#include <tsid/tasks/task-base.hpp> #include <tsid/tasks/task-base.hpp>
#include <tsid/formulations/contact-level.hpp>
#include <memory>
namespace tsid namespace tsid
{ {
...@@ -31,6 +34,23 @@ namespace tsid ...@@ -31,6 +34,23 @@ namespace tsid