GCC Code Coverage Report

Directory: ./
File: src/tasks/task-joint-posture.cpp
Date: 2024-08-26 20:29:39
Exec Total Coverage
Lines: 0 66 0.0%
Branches: 0 188 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 <tsid/tasks/task-joint-posture.hpp>
19 #include "tsid/robots/robot-wrapper.hpp"
20 #include <pinocchio/algorithm/joint-configuration.hpp>
21
22 namespace tsid {
23 namespace tasks {
24 using namespace math;
25 using namespace trajectories;
26 using namespace pinocchio;
27
28 ✗ TaskJointPosture::TaskJointPosture(const std::string& name, RobotWrapper& robot)
29 : TaskMotion(name, robot),
30 ✗ m_ref(robot.nq_actuated(), robot.na()),
31 ✗ m_constraint(name, robot.na(), robot.nv()) {
32 ✗ m_ref_q_augmented = pinocchio::neutral(robot.model());
33 ✗ m_Kp.setZero(robot.na());
34 ✗ m_Kd.setZero(robot.na());
35 ✗ Vector m = Vector::Ones(robot.na());
36 ✗ setMask(m);
37 }
38
39 ✗ const Vector& TaskJointPosture::mask() const { return m_mask; }
40
41 ✗ void TaskJointPosture::mask(const Vector& m) {
42 // std::cerr<<"The method TaskJointPosture::mask is deprecated. Use
43 // TaskJointPosture::setMask instead.\n";
44 ✗ return setMask(m);
45 }
46
47 ✗ void TaskJointPosture::setMask(ConstRefVector m) {
48 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(
49 m.size() == m_robot.na(),
50 "The size of the mask needs to equal " + std::to_string(m_robot.na()));
51 ✗ m_mask = m;
52 ✗ const Vector::Index dim = static_cast<Vector::Index>(m.sum());
53 ✗ Matrix S = Matrix::Zero(dim, m_robot.nv());
54 ✗ m_activeAxes.resize(dim);
55 ✗ unsigned int j = 0;
56 ✗ for (unsigned int i = 0; i < m.size(); i++)
57 ✗ if (m(i) != 0.0) {
58 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(
59 m(i) == 1.0, "Valid mask values are either 0.0 or 1.0 received: " +
60 std::to_string(m(i)));
61 ✗ S(j, m_robot.nv() - m_robot.na() + i) = 1.0;
62 ✗ m_activeAxes(j) = i;
63 ✗ j++;
64 }
65 ✗ m_constraint.resize((unsigned int)dim, m_robot.nv());
66 ✗ m_constraint.setMatrix(S);
67 }
68
69 ✗ int TaskJointPosture::dim() const { return (int)m_mask.sum(); }
70
71 ✗ const Vector& TaskJointPosture::Kp() { return m_Kp; }
72
73 ✗ const Vector& TaskJointPosture::Kd() { return m_Kd; }
74
75 ✗ void TaskJointPosture::Kp(ConstRefVector Kp) {
76 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(Kp.size() == m_robot.na(),
77 "The size of the Kp vector needs to equal " +
78 std::to_string(m_robot.na()));
79 ✗ m_Kp = Kp;
80 }
81
82 ✗ void TaskJointPosture::Kd(ConstRefVector Kd) {
83 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(Kd.size() == m_robot.na(),
84 "The size of the Kd vector needs to equal " +
85 std::to_string(m_robot.na()));
86 ✗ m_Kd = Kd;
87 }
88
89 ✗ void TaskJointPosture::setReference(const TrajectorySample& ref) {
90 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(
91 ref.getValue().size() == m_robot.nq_actuated(),
92 "The size of the reference value vector needs to equal " +
93 std::to_string(m_robot.nq_actuated()));
94 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(
95 ref.getDerivative().size() == m_robot.na(),
96 "The size of the reference value derivative vector needs to equal " +
97 std::to_string(m_robot.na()));
98 ✗ PINOCCHIO_CHECK_INPUT_ARGUMENT(
99 ref.getSecondDerivative().size() == m_robot.na(),
100 "The size of the reference value second derivative vector needs to "
101 "equal " +
102 std::to_string(m_robot.na()));
103 ✗ m_ref = ref;
104 }
105
106 ✗ const TrajectorySample& TaskJointPosture::getReference() const { return m_ref; }
107
108 ✗ const Vector& TaskJointPosture::getDesiredAcceleration() const {
109 ✗ return m_a_des;
110 }
111
112 ✗ Vector TaskJointPosture::getAcceleration(ConstRefVector dv) const {
113 ✗ return m_constraint.matrix() * dv;
114 }
115
116 ✗ const Vector& TaskJointPosture::position_error() const { return m_p_error; }
117
118 ✗ const Vector& TaskJointPosture::velocity_error() const { return m_v_error; }
119
120 ✗ const Vector& TaskJointPosture::position() const { return m_p; }
121
122 ✗ const Vector& TaskJointPosture::velocity() const { return m_v; }
123
124 ✗ const Vector& TaskJointPosture::position_ref() const {
125 ✗ return m_ref.getValue();
126 }
127
128 ✗ const Vector& TaskJointPosture::velocity_ref() const {
129 ✗ return m_ref.getDerivative();
130 }
131
132 ✗ const ConstraintBase& TaskJointPosture::getConstraint() const {
133 ✗ return m_constraint;
134 }
135
136 ✗ const ConstraintBase& TaskJointPosture::compute(const double, ConstRefVector q,
137 ConstRefVector v, Data&) {
138 ✗ m_ref_q_augmented.tail(m_robot.nq_actuated()) = m_ref.getValue();
139
140 // Compute errors
141 ✗ m_p_error = pinocchio::difference(m_robot.model(), m_ref_q_augmented, q)
142 ✗ .tail(m_robot.na());
143
144 ✗ m_v = v.tail(m_robot.na());
145 ✗ m_v_error = m_v - m_ref.getDerivative();
146 ✗ m_a_des = -m_Kp.cwiseProduct(m_p_error) - m_Kd.cwiseProduct(m_v_error) +
147 ✗ m_ref.getSecondDerivative();
148
149 ✗ for (unsigned int i = 0; i < m_activeAxes.size(); i++)
150 ✗ m_constraint.vector()(i) = m_a_des(m_activeAxes(i));
151 ✗ return m_constraint;
152 }
153
154 } // namespace tasks
155 } // namespace tsid
156