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
include/tsid/tasks/task-com-equality.hpp
include/tsid/tasks/task-se3-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-bounds.hpp
include/tsid/tasks/task-joint-bounds.hpp
......@@ -150,6 +151,7 @@ SET(${PROJECT_NAME}_ROBOTS_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-acc-force.hpp
)
......@@ -198,6 +200,7 @@ SET(${PROJECT_NAME}_TASKS_SOURCES
src/tasks/task-com-equality.cpp
src/tasks/task-contact-force-equality.cpp
src/tasks/task-contact-force.cpp
src/tasks/task-cop-equality.cpp
src/tasks/task-joint-bounds.cpp
src/tasks/task-joint-posture.cpp
src/tasks/task-joint-posVelAcc-bounds.cpp
......@@ -232,6 +235,7 @@ SET(${PROJECT_NAME}_ROBOTS_SOURCES
)
SET(${PROJECT_NAME}_FORMULATIONS_SOURCES
src/formulations/contact-level.cpp
src/formulations/inverse-dynamics-formulation-base.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 @@
#include "tsid/tasks/task-joint-posture.hpp"
#include "tsid/tasks/task-se3-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-joint-bounds.hpp"
#include "tsid/tasks/task-angular-momentum-equality.hpp"
......@@ -62,6 +63,7 @@ namespace tsid
.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_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("updateTaskWeight", &InvDynPythonVisitor::updateTaskWeight, bp::args("task_name", "weight"))
.def("updateRigidContactWeights", &InvDynPythonVisitor::updateRigidContactWeights, bp::args("contact_name", "force_regularization_weight"))
......@@ -102,6 +104,9 @@ namespace tsid
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);
}
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){
return self.addActuationTask(task, weight, priorityLevel, transition_duration);
}
......
......@@ -19,6 +19,7 @@
#define __tsid_python_expose_tasks_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-joint-posture.hpp"
#include "tsid/bindings/python/tasks/task-actuation-bounds.hpp"
......@@ -31,6 +32,7 @@ namespace tsid
namespace python
{
void exposeTaskComEquality();
void exposeTaskCopEquality();
void exposeTaskSE3Equality();
void exposeTaskJointPosture();
void exposeTaskActuationBounds();
......@@ -40,6 +42,7 @@ namespace tsid
inline void exposeTasks()
{
exposeTaskComEquality();
exposeTaskCopEquality();
exposeTaskSE3Equality();
exposeTaskJointPosture();
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
typedef math::Vector6 Vector6;
typedef math::Vector3 Vector3;
typedef math::Vector Vector;
typedef tasks::TaskMotion TaskMotion;
typedef tasks::TaskSE3Equality TaskSE3Equality;
typedef math::ConstraintInequality ConstraintInequality;
typedef math::ConstraintEquality ConstraintEquality;
......@@ -87,7 +86,7 @@ namespace tsid
ConstRefVector v,
const Data & data);
const TaskMotion & getMotionTask() const;
const TaskSE3Equality & getMotionTask() const;
const ConstraintBase & getMotionConstraint() const;
const ConstraintInequality & getForceConstraint() const;
const ConstraintEquality & getForceRegularizationTask() const;
......@@ -95,6 +94,7 @@ namespace tsid
double getNormalForce(ConstRefVector f) const;
double getMinNormalForce() const;
double getMaxNormalForce() const;
const Matrix3x & getContactPoints() const;
const Vector & Kp() const;
const Vector & Kd() const;
......
......@@ -20,7 +20,7 @@
#include "tsid/math/fwd.hpp"
#include "tsid/robots/fwd.hpp"
#include "tsid/tasks/task-motion.hpp"
#include "tsid/tasks/task-se3-equality.hpp"
namespace tsid
......@@ -41,7 +41,8 @@ namespace tsid
typedef math::ConstraintEquality ConstraintEquality;
typedef math::ConstRefVector ConstRefVector;
typedef math::Matrix Matrix;
typedef tasks::TaskMotion TaskMotion;
typedef math::Matrix3x Matrix3x;
typedef tasks::TaskSE3Equality TaskSE3Equality;
typedef pinocchio::Data Data;
typedef robots::RobotWrapper RobotWrapper;
......@@ -75,7 +76,7 @@ namespace tsid
ConstRefVector v,
const Data & data) = 0;
virtual const TaskMotion & getMotionTask() const = 0;
virtual const TaskSE3Equality & getMotionTask() const = 0;
virtual const ConstraintBase & getMotionConstraint() const = 0;
virtual const ConstraintInequality & getForceConstraint() const = 0;
virtual const ConstraintEquality & getForceRegularizationTask() const = 0;
......@@ -85,6 +86,7 @@ namespace tsid
virtual bool setMinNormalForce(const double minNormalForce) = 0;
virtual bool setMaxNormalForce(const double maxNormalForce) = 0;
virtual double getNormalForce(ConstRefVector f) const = 0;
virtual const Matrix3x & getContactPoints() const = 0;
protected:
std::string m_name;
......
......@@ -38,7 +38,6 @@ namespace tsid
typedef math::Vector6 Vector6;
typedef math::Vector3 Vector3;
typedef math::Vector Vector;
typedef tasks::TaskMotion TaskMotion;
typedef tasks::TaskSE3Equality TaskSE3Equality;
typedef math::ConstraintInequality ConstraintInequality;
typedef math::ConstraintEquality ConstraintEquality;
......@@ -75,18 +74,19 @@ namespace tsid
ConstRefVector v,
const Data & data);
const TaskMotion & getMotionTask() const;
const TaskSE3Equality & getMotionTask() const;
const ConstraintBase & getMotionConstraint() const;
const ConstraintInequality & getForceConstraint() const;
const ConstraintEquality & getForceRegularizationTask() const;
double getMotionTaskWeight() const;
const Matrix3x & getContactPoints() const;
double getNormalForce(ConstRefVector f) const;
double getMinNormalForce() const;
double getMaxNormalForce() const;
const Vector & Kp() const;
const Vector & Kd() const;
const Vector & Kp(); // cannot be const because it set a member variable inside
const Vector & Kd(); // cannot be const because it set a member variable inside
void Kp(ConstRefVector Kp);
void Kd(ConstRefVector Kp);
......@@ -123,6 +123,8 @@ namespace tsid
Vector3 m_contactNormal;
Vector3 m_fRef;
Vector3 m_weightForceRegTask;
Matrix3x m_contactPoints;
Vector m_Kp3, m_Kd3; // gain vectors to be returned by reference
double m_mu;
double m_fMin;
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
// tsid is free software: you can redistribute it
......@@ -19,38 +19,13 @@
#define __invdyn_inverse_dynamics_formulation_acc_force_hpp__
#include "tsid/formulations/inverse-dynamics-formulation-base.hpp"
#include "tsid/formulations/contact-level.hpp"
#include "tsid/math/constraint-equality.hpp"
#include <vector>
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
{
......@@ -138,7 +113,8 @@ namespace tsid
public:
void addTask(std::shared_ptr<TaskLevel> task,
template<class TaskLevelPointer>
void addTask(TaskLevelPointer task,
double weight,
unsigned int priorityLevel);
......@@ -150,10 +126,10 @@ namespace tsid
Data m_data;
HQPData m_hqpData;
std::vector<std::shared_ptr<TaskLevel> > m_taskMotions;
std::vector<std::shared_ptr<TaskLevel> > m_taskContactForces;
std::vector<std::shared_ptr<TaskLevel> > m_taskActuations;
std::vector<std::shared_ptr<ContactLevel> > m_contacts;
std::vector<std::shared_ptr<TaskLevel> > m_taskMotions;
std::vector<std::shared_ptr<TaskLevelForce> > m_taskContactForces;
std::vector<std::shared_ptr<TaskLevel> > m_taskActuations;
std::vector<std::shared_ptr<ContactLevel> > m_contacts;
double m_t; /// time
unsigned int m_k; /// number of contact-force variables
unsigned int m_v; /// number of acceleration variables
......
......@@ -32,6 +32,30 @@
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
///
......
......@@ -19,6 +19,9 @@
#define __invdyn_task_contact_force_hpp__
#include <tsid/tasks/task-base.hpp>
#include <tsid/formulations/contact-level.hpp>
#include <memory>
namespace tsid
{
......@@ -31,6 +34,23 @@ namespace tsid
TaskContactForce(const std::string & name,
RobotWrapper & robot);
/**
* Contact force tasks have an additional compute method that takes as extra input
* argument the list of active contacts. This can be needed for force tasks that
* involve all contacts, such as the CoP task.
*/
virtual const ConstraintBase & compute(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data,
const std::vector<std::shared_ptr<ContactLevel> > *contacts) = 0;
/**
* Return the name of the contact associated to this task if this task is associated to a specific contact.
* If this task is associated to multiple contact forces (all of them), returns an empty string.
*/
virtual const std::string& getAssociatedContactName() = 0;
};
}
}
......
//
// 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 __invdyn_task_cop_equality_hpp__
#define __invdyn_task_cop_equality_hpp__
#include "tsid/math/fwd.hpp"
#include "tsid/tasks/task-contact-force.hpp"
#include "tsid/trajectories/trajectory-base.hpp"
#include "tsid/math/constraint-equality.hpp"
#include "tsid/formulations/inverse-dynamics-formulation-base.hpp"
namespace tsid
{
namespace tasks
{
class TaskCopEquality : public TaskContactForce
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef math::Index Index;
typedef trajectories::TrajectorySample TrajectorySample;
typedef math::Vector Vector;
typedef math::Vector3 Vector3;
typedef math::ConstraintEquality ConstraintEquality;
typedef pinocchio::SE3 SE3;
TaskCopEquality(const std::string & name,
RobotWrapper & robot);
void setContactList(const std::vector<std::shared_ptr<ContactLevel> > *contacts);
int dim() const;
virtual const std::string& getAssociatedContactName();
const ConstraintBase & compute(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data);
const ConstraintBase & compute(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data,
const std::vector<std::shared_ptr<ContactLevel> > *contacts);
const ConstraintBase & getConstraint() const;
void setReference(const Vector3 & ref);
const Vector3 & getReference() const;
protected:
const std::vector<std::shared_ptr<ContactLevel> > *m_contacts;
std::string m_contact_name; // an empty string
Vector3 m_normal; // normal direction to the ground expressed in world frame
Vector3 m_ref; // reference CoP in world frame
ConstraintEquality m_constraint;
};
}
}
#endif // ifndef __invdyn_task_com_equality_hpp__
......@@ -171,6 +171,11 @@ bool Contact6d::setContactPoints(ConstRefMatrix contactPoints)
return true;
}
const Matrix3x & Contact6d::getContactPoints() const
{
return m_contactPoints;
}
bool Contact6d::setContactNormal(ConstRefVector contactNormal)
{
assert(contactNormal.size()==3);
......@@ -259,7 +264,7 @@ computeForceRegularizationTask(const double ,
double Contact6d::getMinNormalForce() const { return m_fMin; }
double Contact6d::getMaxNormalForce() const { return m_fMax; }
const TaskMotion & Contact6d::getMotionTask() const { return m_motionTask; }
const TaskSE3Equality & Contact6d::getMotionTask() const { return m_motionTask; }
const ConstraintBase & Contact6d::getMotionConstraint() const { return m_motionTask.getConstraint(); }
......
......@@ -45,6 +45,8 @@ ContactPoint::ContactPoint(const std::string & name,
m_weightForceRegTask << 1, 1, 1e-3;
m_forceGenMat.resize(3, 3);
m_fRef = Vector3::Zero();
m_contactPoints.resize(3,1);
m_contactPoints.setZero();
updateForceGeneratorMatrix();
updateForceInequalityConstraints();
updateForceRegularizationTask();
......@@ -94,6 +96,11 @@ double ContactPoint::getNormalForce(ConstRefVector f) const
return m_contactNormal.dot(f);
}
const Matrix3x & ContactPoint::getContactPoints() const
{