GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/tasks/task-contact-force-equality.cpp Lines: 0 66 0.0 %
Date: 2024-05-10 01:36:27 Branches: 0 86 0.0 %

Line Branch Exec Source
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