1 |
|
|
// |
2 |
|
|
// Copyright (c) 2017 CNRS |
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 <Eigen/Dense> |
19 |
|
|
#include <pinocchio/multibody/model.hpp> |
20 |
|
|
#include "tsid/tasks/task-contact-force-equality.hpp" |
21 |
|
|
|
22 |
|
|
namespace tsid { |
23 |
|
|
namespace tasks { |
24 |
|
|
|
25 |
|
|
using namespace tsid::math; |
26 |
|
|
using namespace std; |
27 |
|
|
|
28 |
|
|
TaskContactForceEquality::TaskContactForceEquality( |
29 |
|
|
const std::string& name, RobotWrapper& robot, const double dt, |
30 |
|
|
contacts::ContactBase& contact) |
31 |
|
|
: TaskContactForce(name, robot), |
32 |
|
|
m_contact(&contact), |
33 |
|
|
m_constraint(name, 6, 12), |
34 |
|
|
m_ref(6, 6), |
35 |
|
|
m_fext(6, 6) { |
36 |
|
|
m_forceIntegralError = Vector::Zero(6); |
37 |
|
|
m_dt = dt; |
38 |
|
|
m_leak_rate = 0.05; |
39 |
|
|
m_contact_name = m_contact->name(); |
40 |
|
|
} |
41 |
|
|
|
42 |
|
|
int TaskContactForceEquality::dim() const { return 6; } |
43 |
|
|
|
44 |
|
|
const Vector& TaskContactForceEquality::Kp() const { return m_Kp; } |
45 |
|
|
const Vector& TaskContactForceEquality::Kd() const { return m_Kd; } |
46 |
|
|
const Vector& TaskContactForceEquality::Ki() const { return m_Ki; } |
47 |
|
|
const double& TaskContactForceEquality::getLeakRate() const { |
48 |
|
|
return m_leak_rate; |
49 |
|
|
} |
50 |
|
|
|
51 |
|
|
void TaskContactForceEquality::Kp(ConstRefVector Kp) { |
52 |
|
|
PINOCCHIO_CHECK_INPUT_ARGUMENT(Kp.size() == 6, |
53 |
|
|
"The size of the Kp vector needs to equal 6"); |
54 |
|
|
m_Kp = Kp; |
55 |
|
|
} |
56 |
|
|
|
57 |
|
|
void TaskContactForceEquality::Kd(ConstRefVector Kd) { |
58 |
|
|
PINOCCHIO_CHECK_INPUT_ARGUMENT(Kd.size() == 6, |
59 |
|
|
"The size of the Kd vector needs to equal 6"); |
60 |
|
|
m_Kd = Kd; |
61 |
|
|
} |
62 |
|
|
|
63 |
|
|
void TaskContactForceEquality::Ki(ConstRefVector Ki) { |
64 |
|
|
PINOCCHIO_CHECK_INPUT_ARGUMENT(Ki.size() == 6, |
65 |
|
|
"The size of the Ki vector needs to equal 6"); |
66 |
|
|
m_Ki = Ki; |
67 |
|
|
} |
68 |
|
|
|
69 |
|
|
void TaskContactForceEquality::setLeakRate(double leak) { m_leak_rate = leak; } |
70 |
|
|
|
71 |
|
|
const std::string& TaskContactForceEquality::getAssociatedContactName() { |
72 |
|
|
return m_contact_name; |
73 |
|
|
} |
74 |
|
|
|
75 |
|
|
const contacts::ContactBase& TaskContactForceEquality::getAssociatedContact() { |
76 |
|
|
return *m_contact; |
77 |
|
|
} |
78 |
|
|
|
79 |
|
|
void TaskContactForceEquality::setAssociatedContact( |
80 |
|
|
contacts::ContactBase& contact) { |
81 |
|
|
m_contact = &contact; |
82 |
|
|
m_contact_name = m_contact->name(); |
83 |
|
|
} |
84 |
|
|
|
85 |
|
|
void TaskContactForceEquality::setReference(TrajectorySample& ref) { |
86 |
|
|
m_ref = ref; |
87 |
|
|
} |
88 |
|
|
|
89 |
|
|
const TaskContactForceEquality::TrajectorySample& |
90 |
|
|
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& |
99 |
|
|
TaskContactForceEquality::getExternalForce() const { |
100 |
|
|
return m_fext; |
101 |
|
|
} |
102 |
|
|
|
103 |
|
|
const ConstraintBase& TaskContactForceEquality::compute( |
104 |
|
|
const double t, ConstRefVector q, ConstRefVector v, Data& data, |
105 |
|
|
const std::vector<std::shared_ptr<ContactLevel> >* contacts) { |
106 |
|
|
bool contactFound = false; |
107 |
|
|
if (m_contact_name != "") { |
108 |
|
|
// look if the associated contact is in the list of contact |
109 |
|
|
for (auto cl : *contacts) { |
110 |
|
|
if (m_contact_name == cl->contact.name()) { |
111 |
|
|
contactFound = true; |
112 |
|
|
break; |
113 |
|
|
} |
114 |
|
|
} |
115 |
|
|
} else { |
116 |
|
|
std::cout << "[TaskContactForceEquality] ERROR: Contact name empty" |
117 |
|
|
<< std::endl; |
118 |
|
|
return m_constraint; |
119 |
|
|
} |
120 |
|
|
if (!contactFound) { |
121 |
|
|
std::cout << "[TaskContactForceEquality] ERROR: Contact name not in the " |
122 |
|
|
"list of contact in the formulation pb" |
123 |
|
|
<< std::endl; |
124 |
|
|
return m_constraint; |
125 |
|
|
} |
126 |
|
|
return compute(t, q, v, data); |
127 |
|
|
} |
128 |
|
|
|
129 |
|
|
const ConstraintBase& TaskContactForceEquality::compute(const double, |
130 |
|
|
ConstRefVector, |
131 |
|
|
ConstRefVector, |
132 |
|
|
Data& /*data*/) { |
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 = |
138 |
|
|
m_ref.getValue() + m_Kp.cwiseProduct(forceError) + |
139 |
|
|
m_Kd.cwiseProduct(m_ref.getDerivative() - m_fext.getDerivative()) + |
140 |
|
|
m_Ki.cwiseProduct(m_forceIntegralError); |
141 |
|
|
m_constraint.vector() = f_ref; |
142 |
|
|
|
143 |
|
|
m_forceIntegralError += |
144 |
|
|
(forceError - m_leak_rate * m_forceIntegralError) * m_dt; |
145 |
|
|
|
146 |
|
|
return m_constraint; |
147 |
|
|
} |
148 |
|
|
|
149 |
|
|
const ConstraintBase& TaskContactForceEquality::getConstraint() const { |
150 |
|
|
return m_constraint; |
151 |
|
|
} |
152 |
|
|
|
153 |
|
|
} // namespace tasks |
154 |
|
|
} // namespace tsid |