Skip to content

Commit bce8eb8

Browse files
NoelieRamuzatNoelieRamuzat
authored
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: Noëlie Ramuzat <[email protected]>
1 parent 8ebc861 commit bce8eb8

File tree

4 files changed

+354
-9
lines changed

4 files changed

+354
-9
lines changed

‎bindings/python/tasks/task-contact-force-equality.cpp‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
//
2+
// Copyright (c) 2021 LAAS-CNRS, University of Trento
3+
//
4+
// This file is part of tsid
5+
// tsid is free software: you can redistribute it
6+
// and/or modify it under the terms of the GNU Lesser General Public
7+
// License as published by the Free Software Foundation, either version
8+
// 3 of the License, or (at your option) any later version.
9+
// tsid is distributed in the hope that it will be
10+
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11+
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12+
// General Lesser Public License for more details. You should have
13+
// received a copy of the GNU Lesser General Public License along with
14+
// tsid If not, see
15+
// <http://www.gnu.org/licenses/>.
16+
//
17+
18+
#include "tsid/bindings/python/tasks/task-contact-force-equality.hpp"
19+
#include "tsid/bindings/python/tasks/expose-tasks.hpp"
20+
21+
namespace tsid
22+
{
23+
namespace python
24+
{
25+
void exposeTaskContactForceEquality()
26+
{
27+
TaskContactForceEqualityPythonVisitor<tsid::tasks::TaskContactForceEquality>::expose("TaskContactForceEquality");
28+
}
29+
}
30+
}

‎include/tsid/bindings/python/tasks/task-contact-force-equality.hpp‎

Lines changed: 118 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,118 @@
1+
//
2+
// Copyright (c) 2021 LAAS-CNRS, University of Trento
3+
//
4+
// This file is part of tsid
5+
// tsid is free software: you can redistribute it
6+
// and/or modify it under the terms of the GNU Lesser General Public
7+
// License as published by the Free Software Foundation, either version
8+
// 3 of the License, or (at your option) any later version.
9+
// tsid is distributed in the hope that it will be
10+
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
11+
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12+
// General Lesser Public License for more details. You should have
13+
// received a copy of the GNU Lesser General Public License along with
14+
// tsid If not, see
15+
// <http://www.gnu.org/licenses/>.
16+
//
17+
18+
#ifndef __tsid_python_task_contact_force_equality_hpp__
19+
#define __tsid_python_task_contact_force_equality_hpp__
20+
21+
#include "tsid/bindings/python/fwd.hpp"
22+
23+
#include "tsid/tasks/task-contact-force-equality.hpp"
24+
#include "tsid/robots/robot-wrapper.hpp"
25+
#include "tsid/trajectories/trajectory-base.hpp"
26+
#include "tsid/math/constraint-equality.hpp"
27+
#include "tsid/math/constraint-base.hpp"
28+
namespace tsid
29+
{
30+
namespace python
31+
{
32+
namespace bp = boost::python;
33+
34+
template<typename TaskContactForceEquality>
35+
struct TaskContactForceEqualityPythonVisitor
36+
: public boost::python::def_visitor< TaskContactForceEqualityPythonVisitor<TaskContactForceEquality> >
37+
{
38+
39+
template<class PyClass>
40+
41+
42+
void visit(PyClass& cl) const
43+
{
44+
cl
45+
.def(bp::init<std::string, robots::RobotWrapper &, double, contacts::ContactBase &> ((bp::arg("name"), bp::arg("robot"), bp::arg("dt"), bp::arg("contact")), "Default Constructor"))
46+
.add_property("dim", &TaskContactForceEquality::dim, "return dimension size")
47+
.def("setReference", &TaskContactForceEqualityPythonVisitor::setReference, bp::arg("ref"))
48+
.def("setExternalForce", &TaskContactForceEqualityPythonVisitor::setExternalForce, bp::arg("f_ext"))
49+
.def("compute", &TaskContactForceEqualityPythonVisitor::compute, bp::args("t", "q", "v", "data"))
50+
.def("getConstraint", &TaskContactForceEqualityPythonVisitor::getConstraint)
51+
.add_property("name", &TaskContactForceEqualityPythonVisitor::name)
52+
.add_property("Kp", bp::make_function(&TaskContactForceEqualityPythonVisitor::Kp, bp::return_value_policy<bp::copy_const_reference>()))
53+
.add_property("Kd", bp::make_function(&TaskContactForceEqualityPythonVisitor::Kd, bp::return_value_policy<bp::copy_const_reference>()))
54+
.add_property("Ki", bp::make_function(&TaskContactForceEqualityPythonVisitor::Kd, bp::return_value_policy<bp::copy_const_reference>()))
55+
.add_property("getLeakRate", bp::make_function(&TaskContactForceEqualityPythonVisitor::getLeakRate, bp::return_value_policy<bp::copy_const_reference>()))
56+
.def("setKp", &TaskContactForceEqualityPythonVisitor::setKp, bp::arg("Kp"))
57+
.def("setKd", &TaskContactForceEqualityPythonVisitor::setKd, bp::arg("Kd"))
58+
.def("setKi", &TaskContactForceEqualityPythonVisitor::setKi, bp::arg("Ki"))
59+
.def("setLeakRate", &TaskContactForceEqualityPythonVisitor::setLeakRate, bp::arg("leak"))
60+
;
61+
}
62+
static std::string name(TaskContactForceEquality & self){
63+
std::string name = self.name();
64+
return name;
65+
}
66+
static math::ConstraintEquality compute(TaskContactForceEquality & self, const double t, const Eigen::VectorXd & q, const Eigen::VectorXd & v, pinocchio::Data & data){
67+
self.compute(t, q, v, data);
68+
math::ConstraintEquality cons(self.getConstraint().name(), self.getConstraint().matrix(), self.getConstraint().vector());
69+
return cons;
70+
}
71+
static math::ConstraintEquality getConstraint(const TaskContactForceEquality & self){
72+
math::ConstraintEquality cons(self.getConstraint().name(), self.getConstraint().matrix(), self.getConstraint().vector());
73+
return cons;
74+
}
75+
static void setReference(TaskContactForceEquality & self, trajectories::TrajectorySample & ref){
76+
self.setReference(ref);
77+
}
78+
static void setExternalForce(TaskContactForceEquality & self, trajectories::TrajectorySample & f_ext){
79+
self.setExternalForce(f_ext);
80+
}
81+
static const Eigen::VectorXd & Kp (TaskContactForceEquality & self){
82+
return self.Kp();
83+
}
84+
static const Eigen::VectorXd & Kd (TaskContactForceEquality & self){
85+
return self.Kd();
86+
}
87+
static const Eigen::VectorXd & Ki (TaskContactForceEquality & self){
88+
return self.Ki();
89+
}
90+
static const double & getLeakRate (TaskContactForceEquality & self){
91+
return self.getLeakRate();
92+
}
93+
static void setKp (TaskContactForceEquality & self, const::Eigen::VectorXd Kp){
94+
return self.Kp(Kp);
95+
}
96+
static void setKd (TaskContactForceEquality & self, const::Eigen::VectorXd Kd){
97+
return self.Kd(Kd);
98+
}
99+
static void setKi (TaskContactForceEquality & self, const::Eigen::VectorXd Ki){
100+
return self.Ki(Ki);
101+
}
102+
static void setLeakRate (TaskContactForceEquality & self, const double leak){
103+
return self.setLeakRate(leak);
104+
}
105+
static void expose(const std::string & class_name)
106+
{
107+
std::string doc = "TaskContactForceEqualityPythonVisitor info.";
108+
bp::class_<TaskContactForceEquality>(class_name.c_str(),
109+
doc.c_str(),
110+
bp::no_init)
111+
.def(TaskContactForceEqualityPythonVisitor<TaskContactForceEquality>());
112+
}
113+
};
114+
}
115+
}
116+
117+
118+
#endif // ifndef __tsid_python_task_contact_force_equality_hpp__

‎include/tsid/tasks/task-contact-force-equality.hpp‎

Lines changed: 78 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -15,15 +15,89 @@
1515
// <http://www.gnu.org/licenses/>.
1616
//
1717

18-
#ifndef __invdyn_tasks_fwd_hpp__
19-
#define __invdyn_tasks_fwd_hpp__
18+
#ifndef __invdyn_task_contact_force_equality_hpp__
19+
#define __invdyn_task_contact_force_equality_hpp__
20+
21+
#include "tsid/math/fwd.hpp"
22+
#include "tsid/tasks/task-contact-force.hpp"
23+
#include "tsid/trajectories/trajectory-base.hpp"
24+
#include "tsid/math/constraint-equality.hpp"
25+
#include "tsid/contacts/contact-base.hpp"
2026

2127
namespace tsid
2228
{
2329
namespace tasks
2430
{
25-
31+
32+
class TaskContactForceEquality : public TaskContactForce
33+
{
34+
public:
35+
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
36+
37+
typedef math::Index Index;
38+
typedef trajectories::TrajectorySample TrajectorySample;
39+
typedef math::Vector Vector;
40+
typedef math::Vector6 Vector6;
41+
typedef math::Vector3 Vector3;
42+
typedef math::ConstraintEquality ConstraintEquality;
43+
typedef pinocchio::SE3 SE3;
44+
45+
TaskContactForceEquality(const std::string & name,
46+
RobotWrapper & robot,
47+
const double dt,
48+
contacts::ContactBase & contact);
49+
50+
int dim() const;
51+
52+
virtual const std::string& getAssociatedContactName();
53+
virtual const contacts::ContactBase& getAssociatedContact();
54+
void setAssociatedContact(contacts::ContactBase & contact);
55+
56+
// Task expressed as a PID between the reference force and the external one
57+
const ConstraintBase & compute(const double t,
58+
ConstRefVector q,
59+
ConstRefVector v,
60+
Data & data);
61+
62+
const ConstraintBase & compute(const double t,
63+
ConstRefVector q,
64+
ConstRefVector v,
65+
Data & data,
66+
const std::vector<std::shared_ptr<ContactLevel> > *contacts);
67+
68+
const ConstraintBase & getConstraint() const;
69+
70+
void setReference(TrajectorySample & ref);
71+
const TrajectorySample & getReference() const;
72+
73+
void setExternalForce(TrajectorySample & f_ext);
74+
const TrajectorySample & getExternalForce() const;
75+
76+
const Vector & Kp() const;
77+
const Vector & Kd() const;
78+
const Vector & Ki() const;
79+
const double & getLeakRate() const;
80+
void Kp(ConstRefVector Kp);
81+
void Kd(ConstRefVector Kp);
82+
void Ki(ConstRefVector Ki);
83+
void setLeakRate(double leak);
84+
85+
protected:
86+
// contact associated to the force task
87+
contacts::ContactBase * m_contact;
88+
std::string m_contact_name; // the associated contact name or an empty string
89+
ConstraintEquality m_constraint;
90+
TrajectorySample m_ref; // reference Force 6D to follow
91+
TrajectorySample m_fext; // external Force 6D in the same frame than the ref
92+
Vector m_forceIntegralError; // Integral error of the PID
93+
Vector m_Kp;
94+
Vector m_Kd;
95+
Vector m_Ki;
96+
double m_dt;
97+
double m_leak_rate;
98+
};
99+
26100
}
27101
}
28102

29-
#endif // ifndef __invdyn_tasks_fwd_hpp__
103+
#endif // ifndef __invdyn_task_contact_force_equality_hpp__

‎src/tasks/task-contact-force-equality.cpp‎

Lines changed: 128 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -17,12 +17,135 @@
1717

1818
#include <Eigen/Dense>
1919
#include <pinocchio/multibody/model.hpp>
20+
#include "tsid/tasks/task-contact-force-equality.hpp"
2021

21-
namespace tsid
22+
namespace tsid {
23+
namespace tasks {
24+
25+
using namespace tsid::math;
26+
using namespace std;
27+
28+
TaskContactForceEquality::TaskContactForceEquality(const std::string & name, RobotWrapper & robot,
29+
const double dt, contacts::ContactBase & contact):
30+
TaskContactForce(name, robot),
31+
m_contact(&contact),
32+
m_constraint(name, 6, 12),
33+
m_ref(6,6),
34+
m_fext(6,6) {
35+
m_forceIntegralError = Vector::Zero(6);
36+
m_dt = dt;
37+
m_leak_rate = 0.05;
38+
m_contact_name = m_contact->name();
39+
}
40+
41+
int TaskContactForceEquality::dim() const {
42+
return 6;
43+
}
44+
45+
const Vector & TaskContactForceEquality::Kp() const { return m_Kp; }
46+
const Vector & TaskContactForceEquality::Kd() const { return m_Kd; }
47+
const Vector & TaskContactForceEquality::Ki() const { return m_Ki; }
48+
const double & TaskContactForceEquality::getLeakRate() const { return m_leak_rate; }
49+
50+
void TaskContactForceEquality::Kp(ConstRefVector Kp)
51+
{
52+
assert(Kp.size()==6);
53+
m_Kp = Kp;
54+
}
55+
56+
void TaskContactForceEquality::Kd(ConstRefVector Kd)
2257
{
23-
namespace tasks
24-
{
25-
26-
58+
assert(Kd.size()==6);
59+
m_Kd = Kd;
60+
}
61+
62+
void TaskContactForceEquality::Ki(ConstRefVector Ki)
63+
{
64+
assert(Ki.size()==6);
65+
m_Ki = Ki;
66+
}
67+
68+
void TaskContactForceEquality::setLeakRate(double leak)
69+
{
70+
m_leak_rate = leak;
71+
}
72+
73+
const std::string& TaskContactForceEquality::getAssociatedContactName() {
74+
return m_contact_name;
75+
}
76+
77+
const contacts::ContactBase& TaskContactForceEquality::getAssociatedContact() {
78+
return *m_contact;
79+
}
80+
81+
void TaskContactForceEquality::setAssociatedContact(contacts::ContactBase & contact) {
82+
m_contact = &contact;
83+
m_contact_name = m_contact->name();
84+
}
85+
86+
void TaskContactForceEquality::setReference(TrajectorySample & ref) {
87+
m_ref = ref;
88+
}
89+
90+
const TaskContactForceEquality::TrajectorySample & TaskContactForceEquality::getReference() const {
91+
return m_ref;
92+
}
93+
94+
void TaskContactForceEquality::setExternalForce(TrajectorySample & f_ext) {
95+
m_fext = f_ext;
96+
}
97+
98+
const TaskContactForceEquality::TrajectorySample & TaskContactForceEquality::getExternalForce() const {
99+
return m_fext;
100+
}
101+
102+
103+
const ConstraintBase & TaskContactForceEquality::compute(const double t,
104+
ConstRefVector q,
105+
ConstRefVector v,
106+
Data & data,
107+
const std::vector<std::shared_ptr<ContactLevel> > *contacts) {
108+
bool contactFound = false;
109+
if (m_contact_name != "") {
110+
// look if the associated contact is in the list of contact
111+
for(auto cl : *contacts) {
112+
if (m_contact_name == cl->contact.name()) {
113+
contactFound = true;
114+
break;
115+
}
116+
}
117+
} else {
118+
std::cout << "[TaskContactForceEquality] ERROR: Contact name empty" << std::endl;
119+
return m_constraint;
120+
}
121+
if (!contactFound) {
122+
std::cout << "[TaskContactForceEquality] ERROR: Contact name not in the list of contact in the formulation pb" << std::endl;
123+
return m_constraint;
27124
}
125+
return compute(t, q, v, data);
126+
}
127+
128+
const ConstraintBase & TaskContactForceEquality::compute(const double,
129+
ConstRefVector,
130+
ConstRefVector,
131+
Data & data) {
132+
133+
auto& M = m_constraint.matrix();
134+
M = m_contact->getForceGeneratorMatrix(); // 6x12 for a 6d contact
135+
136+
Vector forceError = m_ref.getValue() - m_fext.getValue();
137+
Vector f_ref = m_ref.getValue() + m_Kp.cwiseProduct(forceError) + m_Kd.cwiseProduct(m_ref.getDerivative() - m_fext.getDerivative())
138+
+ m_Ki.cwiseProduct(m_forceIntegralError);
139+
m_constraint.vector() = f_ref;
140+
141+
m_forceIntegralError += (forceError - m_leak_rate * m_forceIntegralError) * m_dt;
142+
143+
return m_constraint;
144+
}
145+
146+
const ConstraintBase & TaskContactForceEquality::getConstraint() const {
147+
return m_constraint;
148+
}
149+
150+
}
28151
}

0 commit comments

Comments
 (0)