Unverified Commit bce8eb8b authored by Noëlie RAMUZAT's avatar Noëlie RAMUZAT Committed by GitHub
Browse files

Add implementation of TaskContactForceEquality (#141)



* [ForceTask] Add bindings and set task for contactPoint (3D), use Contact6D, put dt as an input in the constructor & add feedforward, add leak_rate parameter, change constructor & methods to take the contact
Authored-by: default avatarNoëlie Ramuzat <noelie.ramuzat@laas.fr>
parent 8ebc8611
Pipeline #16073 passed with stage
in 13 minutes and 17 seconds
//
// Copyright (c) 2021 LAAS-CNRS, 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-contact-force-equality.hpp"
#include "tsid/bindings/python/tasks/expose-tasks.hpp"
namespace tsid
{
namespace python
{
void exposeTaskContactForceEquality()
{
TaskContactForceEqualityPythonVisitor<tsid::tasks::TaskContactForceEquality>::expose("TaskContactForceEquality");
}
}
}
//
// Copyright (c) 2021 LAAS-CNRS, 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_contact_force_equality_hpp__
#define __tsid_python_task_contact_force_equality_hpp__
#include "tsid/bindings/python/fwd.hpp"
#include "tsid/tasks/task-contact-force-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 TaskContactForceEquality>
struct TaskContactForceEqualityPythonVisitor
: public boost::python::def_visitor< TaskContactForceEqualityPythonVisitor<TaskContactForceEquality> >
{
template<class PyClass>
void visit(PyClass& cl) const
{
cl
.def(bp::init<std::string, robots::RobotWrapper &, double, contacts::ContactBase &> ((bp::arg("name"), bp::arg("robot"), bp::arg("dt"), bp::arg("contact")), "Default Constructor"))
.add_property("dim", &TaskContactForceEquality::dim, "return dimension size")
.def("setReference", &TaskContactForceEqualityPythonVisitor::setReference, bp::arg("ref"))
.def("setExternalForce", &TaskContactForceEqualityPythonVisitor::setExternalForce, bp::arg("f_ext"))
.def("compute", &TaskContactForceEqualityPythonVisitor::compute, bp::args("t", "q", "v", "data"))
.def("getConstraint", &TaskContactForceEqualityPythonVisitor::getConstraint)
.add_property("name", &TaskContactForceEqualityPythonVisitor::name)
.add_property("Kp", bp::make_function(&TaskContactForceEqualityPythonVisitor::Kp, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("Kd", bp::make_function(&TaskContactForceEqualityPythonVisitor::Kd, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("Ki", bp::make_function(&TaskContactForceEqualityPythonVisitor::Kd, bp::return_value_policy<bp::copy_const_reference>()))
.add_property("getLeakRate", bp::make_function(&TaskContactForceEqualityPythonVisitor::getLeakRate, bp::return_value_policy<bp::copy_const_reference>()))
.def("setKp", &TaskContactForceEqualityPythonVisitor::setKp, bp::arg("Kp"))
.def("setKd", &TaskContactForceEqualityPythonVisitor::setKd, bp::arg("Kd"))
.def("setKi", &TaskContactForceEqualityPythonVisitor::setKi, bp::arg("Ki"))
.def("setLeakRate", &TaskContactForceEqualityPythonVisitor::setLeakRate, bp::arg("leak"))
;
}
static std::string name(TaskContactForceEquality & self){
std::string name = self.name();
return name;
}
static math::ConstraintEquality compute(TaskContactForceEquality & 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 TaskContactForceEquality & self){
math::ConstraintEquality cons(self.getConstraint().name(), self.getConstraint().matrix(), self.getConstraint().vector());
return cons;
}
static void setReference(TaskContactForceEquality & self, trajectories::TrajectorySample & ref){
self.setReference(ref);
}
static void setExternalForce(TaskContactForceEquality & self, trajectories::TrajectorySample & f_ext){
self.setExternalForce(f_ext);
}
static const Eigen::VectorXd & Kp (TaskContactForceEquality & self){
return self.Kp();
}
static const Eigen::VectorXd & Kd (TaskContactForceEquality & self){
return self.Kd();
}
static const Eigen::VectorXd & Ki (TaskContactForceEquality & self){
return self.Ki();
}
static const double & getLeakRate (TaskContactForceEquality & self){
return self.getLeakRate();
}
static void setKp (TaskContactForceEquality & self, const::Eigen::VectorXd Kp){
return self.Kp(Kp);
}
static void setKd (TaskContactForceEquality & self, const::Eigen::VectorXd Kd){
return self.Kd(Kd);
}
static void setKi (TaskContactForceEquality & self, const::Eigen::VectorXd Ki){
return self.Ki(Ki);
}
static void setLeakRate (TaskContactForceEquality & self, const double leak){
return self.setLeakRate(leak);
}
static void expose(const std::string & class_name)
{
std::string doc = "TaskContactForceEqualityPythonVisitor info.";
bp::class_<TaskContactForceEquality>(class_name.c_str(),
doc.c_str(),
bp::no_init)
.def(TaskContactForceEqualityPythonVisitor<TaskContactForceEquality>());
}
};
}
}
#endif // ifndef __tsid_python_task_contact_force_equality_hpp__
......@@ -15,15 +15,89 @@
// <http://www.gnu.org/licenses/>.
//
#ifndef __invdyn_tasks_fwd_hpp__
#define __invdyn_tasks_fwd_hpp__
#ifndef __invdyn_task_contact_force_equality_hpp__
#define __invdyn_task_contact_force_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/contacts/contact-base.hpp"
namespace tsid
{
namespace tasks
{
class TaskContactForceEquality : public TaskContactForce
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
typedef math::Index Index;
typedef trajectories::TrajectorySample TrajectorySample;
typedef math::Vector Vector;
typedef math::Vector6 Vector6;
typedef math::Vector3 Vector3;
typedef math::ConstraintEquality ConstraintEquality;
typedef pinocchio::SE3 SE3;
TaskContactForceEquality(const std::string & name,
RobotWrapper & robot,
const double dt,
contacts::ContactBase & contact);
int dim() const;
virtual const std::string& getAssociatedContactName();
virtual const contacts::ContactBase& getAssociatedContact();
void setAssociatedContact(contacts::ContactBase & contact);
// Task expressed as a PID between the reference force and the external one
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(TrajectorySample & ref);
const TrajectorySample & getReference() const;
void setExternalForce(TrajectorySample & f_ext);
const TrajectorySample & getExternalForce() const;
const Vector & Kp() const;
const Vector & Kd() const;
const Vector & Ki() const;
const double & getLeakRate() const;
void Kp(ConstRefVector Kp);
void Kd(ConstRefVector Kp);
void Ki(ConstRefVector Ki);
void setLeakRate(double leak);
protected:
// contact associated to the force task
contacts::ContactBase * m_contact;
std::string m_contact_name; // the associated contact name or an empty string
ConstraintEquality m_constraint;
TrajectorySample m_ref; // reference Force 6D to follow
TrajectorySample m_fext; // external Force 6D in the same frame than the ref
Vector m_forceIntegralError; // Integral error of the PID
Vector m_Kp;
Vector m_Kd;
Vector m_Ki;
double m_dt;
double m_leak_rate;
};
}
}
#endif // ifndef __invdyn_tasks_fwd_hpp__
#endif // ifndef __invdyn_task_contact_force_equality_hpp__
......@@ -17,12 +17,135 @@
#include <Eigen/Dense>
#include <pinocchio/multibody/model.hpp>
#include "tsid/tasks/task-contact-force-equality.hpp"
namespace tsid
namespace tsid {
namespace tasks {
using namespace tsid::math;
using namespace std;
TaskContactForceEquality::TaskContactForceEquality(const std::string & name, RobotWrapper & robot,
const double dt, contacts::ContactBase & contact):
TaskContactForce(name, robot),
m_contact(&contact),
m_constraint(name, 6, 12),
m_ref(6,6),
m_fext(6,6) {
m_forceIntegralError = Vector::Zero(6);
m_dt = dt;
m_leak_rate = 0.05;
m_contact_name = m_contact->name();
}
int TaskContactForceEquality::dim() const {
return 6;
}
const Vector & TaskContactForceEquality::Kp() const { return m_Kp; }
const Vector & TaskContactForceEquality::Kd() const { return m_Kd; }
const Vector & TaskContactForceEquality::Ki() const { return m_Ki; }
const double & TaskContactForceEquality::getLeakRate() const { return m_leak_rate; }
void TaskContactForceEquality::Kp(ConstRefVector Kp)
{
assert(Kp.size()==6);
m_Kp = Kp;
}
void TaskContactForceEquality::Kd(ConstRefVector Kd)
{
namespace tasks
{
assert(Kd.size()==6);
m_Kd = Kd;
}
void TaskContactForceEquality::Ki(ConstRefVector Ki)
{
assert(Ki.size()==6);
m_Ki = Ki;
}
void TaskContactForceEquality::setLeakRate(double leak)
{
m_leak_rate = leak;
}
const std::string& TaskContactForceEquality::getAssociatedContactName() {
return m_contact_name;
}
const contacts::ContactBase& TaskContactForceEquality::getAssociatedContact() {
return *m_contact;
}
void TaskContactForceEquality::setAssociatedContact(contacts::ContactBase & contact) {
m_contact = &contact;
m_contact_name = m_contact->name();
}
void TaskContactForceEquality::setReference(TrajectorySample & ref) {
m_ref = ref;
}
const TaskContactForceEquality::TrajectorySample & TaskContactForceEquality::getReference() const {
return m_ref;
}
void TaskContactForceEquality::setExternalForce(TrajectorySample & f_ext) {
m_fext = f_ext;
}
const TaskContactForceEquality::TrajectorySample & TaskContactForceEquality::getExternalForce() const {
return m_fext;
}
const ConstraintBase & TaskContactForceEquality::compute(const double t,
ConstRefVector q,
ConstRefVector v,
Data & data,
const std::vector<std::shared_ptr<ContactLevel> > *contacts) {
bool contactFound = false;
if (m_contact_name != "") {
// look if the associated contact is in the list of contact
for(auto cl : *contacts) {
if (m_contact_name == cl->contact.name()) {
contactFound = true;
break;
}
}
} else {
std::cout << "[TaskContactForceEquality] ERROR: Contact name empty" << std::endl;
return m_constraint;
}
if (!contactFound) {
std::cout << "[TaskContactForceEquality] ERROR: Contact name not in the list of contact in the formulation pb" << std::endl;
return m_constraint;
}
return compute(t, q, v, data);
}
const ConstraintBase & TaskContactForceEquality::compute(const double,
ConstRefVector,
ConstRefVector,
Data & data) {
auto& M = m_constraint.matrix();
M = m_contact->getForceGeneratorMatrix(); // 6x12 for a 6d contact
Vector forceError = m_ref.getValue() - m_fext.getValue();
Vector f_ref = m_ref.getValue() + m_Kp.cwiseProduct(forceError) + m_Kd.cwiseProduct(m_ref.getDerivative() - m_fext.getDerivative())
+ m_Ki.cwiseProduct(m_forceIntegralError);
m_constraint.vector() = f_ref;
m_forceIntegralError += (forceError - m_leak_rate * m_forceIntegralError) * m_dt;
return m_constraint;
}
const ConstraintBase & TaskContactForceEquality::getConstraint() const {
return m_constraint;
}
}
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment