Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/tasks/task-se3-equality.cpp	Lines:	0	110	0.0 %
Date:	2024-02-02 08:47:34	Branches:	0	296	0.0 %


//
// Copyright (c) 2017-2020 CNRS, NYU, MPI Tübingen, Inria
//
// 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/math/utils.hpp"
#include "tsid/tasks/task-se3-equality.hpp"
#include "tsid/robots/robot-wrapper.hpp"

namespace tsid {
namespace tasks {
using namespace std;
using namespace math;
using namespace trajectories;
using namespace pinocchio;

TaskSE3Equality::TaskSE3Equality(const std::string& name, RobotWrapper& robot,
                                 const std::string& frameName)
    : TaskMotion(name, robot),
      m_frame_name(frameName),
      m_constraint(name, 6, robot.nv()),
      m_ref(12, 6) {
  PINOCCHIO_CHECK_INPUT_ARGUMENT(
      m_robot.model().existFrame(frameName),
      "The frame with name '" + frameName + "' does not exist");
  m_frame_id = m_robot.model().getFrameId(frameName);

  m_v_ref.setZero();
  m_a_ref.setZero();
  m_M_ref.setIdentity();
  m_wMl.setIdentity();
  m_p_error_vec.setZero(6);
  m_v_error_vec.setZero(6);
  m_p.resize(12);
  m_v.resize(6);
  m_p_ref.resize(12);
  m_v_ref_vec.resize(6);
  m_Kp.setZero(6);
  m_Kd.setZero(6);
  m_a_des.setZero(6);
  m_J.setZero(6, robot.nv());
  m_J_rotated.setZero(6, robot.nv());

  m_mask.resize(6);
  m_mask.fill(1.);
  setMask(m_mask);

  m_local_frame = true;
}

void TaskSE3Equality::setMask(math::ConstRefVector mask) {
  TaskMotion::setMask(mask);
  int n = dim();
  m_constraint.resize(n, (unsigned int)m_J.cols());
  m_p_error_masked_vec.resize(n);
  m_v_error_masked_vec.resize(n);
  m_drift_masked.resize(n);
  m_a_des_masked.resize(n);
}

int TaskSE3Equality::dim() const { return (int)m_mask.sum(); }

const Vector& TaskSE3Equality::Kp() const { return m_Kp; }

const Vector& TaskSE3Equality::Kd() const { return m_Kd; }

void TaskSE3Equality::Kp(ConstRefVector Kp) {
  PINOCCHIO_CHECK_INPUT_ARGUMENT(Kp.size() == 6,
                                 "The size of the Kp vector needs to equal 6");
  m_Kp = Kp;
}

void TaskSE3Equality::Kd(ConstRefVector Kd) {
  PINOCCHIO_CHECK_INPUT_ARGUMENT(Kd.size() == 6,
                                 "The size of the Kd vector needs to equal 6");
  m_Kd = Kd;
}

void TaskSE3Equality::setReference(TrajectorySample& ref) {
  m_ref = ref;
  TSID_DISABLE_WARNING_PUSH
  TSID_DISABLE_WARNING_DEPRECATED
  PINOCCHIO_CHECK_INPUT_ARGUMENT(
      ref.pos.size() == 12, "The size of the reference vector needs to be 12");
  m_M_ref.translation(ref.pos.head<3>());
  m_M_ref.rotation(MapMatrix3(&ref.pos(3), 3, 3));
  TSID_DISABLE_WARNING_POP
  m_v_ref = Motion(ref.getDerivative());
  m_a_ref = Motion(ref.getSecondDerivative());
}

void TaskSE3Equality::setReference(const SE3& ref) {
  TrajectorySample s(12, 6);
  TSID_DISABLE_WARNING_PUSH
  TSID_DISABLE_WARNING_DEPRECATED
  tsid::math::SE3ToVector(ref, s.pos);
  TSID_DISABLE_WARNING_POP
  setReference(s);
}

const TrajectorySample& TaskSE3Equality::getReference() const { return m_ref; }

const Vector& TaskSE3Equality::position_error() const {
  return m_p_error_masked_vec;
}

const Vector& TaskSE3Equality::velocity_error() const {
  return m_v_error_masked_vec;
}

const Vector& TaskSE3Equality::position() const { return m_p; }

const Vector& TaskSE3Equality::velocity() const { return m_v; }

const Vector& TaskSE3Equality::position_ref() const { return m_p_ref; }

const Vector& TaskSE3Equality::velocity_ref() const { return m_v_ref_vec; }

const Vector& TaskSE3Equality::getDesiredAcceleration() const {
  return m_a_des_masked;
}

Vector TaskSE3Equality::getAcceleration(ConstRefVector dv) const {
  return m_constraint.matrix() * dv + m_drift_masked;
}

Index TaskSE3Equality::frame_id() const { return m_frame_id; }

const ConstraintBase& TaskSE3Equality::getConstraint() const {
  return m_constraint;
}

void TaskSE3Equality::useLocalFrame(bool local_frame) {
  m_local_frame = local_frame;
}

const ConstraintBase& TaskSE3Equality::compute(const double, ConstRefVector,
                                               ConstRefVector, Data& data) {
  SE3 oMi;
  Motion v_frame;
  m_robot.framePosition(data, m_frame_id, oMi);
  m_robot.frameVelocity(data, m_frame_id, v_frame);
  m_robot.frameClassicAcceleration(data, m_frame_id, m_drift);

  // @todo Since Jacobian computation is cheaper in world frame
  // we could do all computations in world frame
  m_robot.frameJacobianLocal(data, m_frame_id, m_J);

  errorInSE3(oMi, m_M_ref, m_p_error);  // pos err in local frame
  SE3ToVector(m_M_ref, m_p_ref);
  SE3ToVector(oMi, m_p);

  // Transformation from local to world
  m_wMl.rotation(oMi.rotation());

  if (m_local_frame) {
    m_p_error_vec = m_p_error.toVector();
    m_v_error = m_wMl.actInv(m_v_ref) - v_frame;  // vel err in local frame

    // desired acc in local frame
    m_a_des = m_Kp.cwiseProduct(m_p_error_vec) +
              m_Kd.cwiseProduct(m_v_error.toVector()) +
              m_wMl.actInv(m_a_ref).toVector();
  } else {
    m_p_error_vec =
        m_wMl.toActionMatrix() *  // pos err in local world-oriented frame
        m_p_error.toVector();

    // cout<<"m_p_error_vec="<<m_p_error_vec.head<3>().transpose()<<endl;
    // cout<<"oMi-m_M_ref
    // ="<<-(oMi.translation()-m_M_ref.translation()).transpose()<<endl;

    m_v_error =
        m_v_ref - m_wMl.act(v_frame);  // vel err in local world-oriented frame

    m_drift = m_wMl.act(m_drift);

    // desired acc in local world-oriented frame
    m_a_des = m_Kp.cwiseProduct(m_p_error_vec) +
              m_Kd.cwiseProduct(m_v_error.toVector()) + m_a_ref.toVector();

    // Use an explicit temporary `m_J_rotated` here to avoid allocations.
    m_J_rotated.noalias() = m_wMl.toActionMatrix() * m_J;
    m_J = m_J_rotated;
  }

  m_v_error_vec = m_v_error.toVector();
  m_v_ref_vec = m_v_ref.toVector();
  m_v = v_frame.toVector();

  int idx = 0;
  for (int i = 0; i < 6; i++) {
    if (m_mask(i) != 1.) continue;

    m_constraint.matrix().row(idx) = m_J.row(i);
    m_constraint.vector().row(idx) = (m_a_des - m_drift.toVector()).row(i);
    m_a_des_masked(idx) = m_a_des(i);
    m_drift_masked(idx) = m_drift.toVector()(i);
    m_p_error_masked_vec(idx) = m_p_error_vec(i);
    m_v_error_masked_vec(idx) = m_v_error_vec(i);

    idx += 1;
  }

  return m_constraint;
}
}  // namespace tasks
}  // namespace tsid


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2017-2020 CNRS, NYU, MPI Tübingen, Inria
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/math/utils.hpp"
19			#include "tsid/tasks/task-se3-equality.hpp"
20			#include "tsid/robots/robot-wrapper.hpp"
21
22			namespace tsid {
23			namespace tasks {
24			using namespace std;
25			using namespace math;
26			using namespace trajectories;
27			using namespace pinocchio;
28
29			TaskSE3Equality::TaskSE3Equality(const std::string& name, RobotWrapper& robot,
30			const std::string& frameName)
31			: TaskMotion(name, robot),
32			m_frame_name(frameName),
33			m_constraint(name, 6, robot.nv()),
34			m_ref(12, 6) {
35			PINOCCHIO_CHECK_INPUT_ARGUMENT(
36			m_robot.model().existFrame(frameName),
37			"The frame with name '" + frameName + "' does not exist");
38			m_frame_id = m_robot.model().getFrameId(frameName);
39
40			m_v_ref.setZero();
41			m_a_ref.setZero();
42			m_M_ref.setIdentity();
43			m_wMl.setIdentity();
44			m_p_error_vec.setZero(6);
45			m_v_error_vec.setZero(6);
46			m_p.resize(12);
47			m_v.resize(6);
48			m_p_ref.resize(12);
49			m_v_ref_vec.resize(6);
50			m_Kp.setZero(6);
51			m_Kd.setZero(6);
52			m_a_des.setZero(6);
53			m_J.setZero(6, robot.nv());
54			m_J_rotated.setZero(6, robot.nv());
55
56			m_mask.resize(6);
57			m_mask.fill(1.);
58			setMask(m_mask);
59
60			m_local_frame = true;
61			}
62
63			void TaskSE3Equality::setMask(math::ConstRefVector mask) {
64			TaskMotion::setMask(mask);
65			int n = dim();
66			m_constraint.resize(n, (unsigned int)m_J.cols());
67			m_p_error_masked_vec.resize(n);
68			m_v_error_masked_vec.resize(n);
69			m_drift_masked.resize(n);
70			m_a_des_masked.resize(n);
71			}
72
73			int TaskSE3Equality::dim() const { return (int)m_mask.sum(); }
74
75			const Vector& TaskSE3Equality::Kp() const { return m_Kp; }
76
77			const Vector& TaskSE3Equality::Kd() const { return m_Kd; }
78
79			void TaskSE3Equality::Kp(ConstRefVector Kp) {
80			PINOCCHIO_CHECK_INPUT_ARGUMENT(Kp.size() == 6,
81			"The size of the Kp vector needs to equal 6");
82			m_Kp = Kp;
83			}
84
85			void TaskSE3Equality::Kd(ConstRefVector Kd) {
86			PINOCCHIO_CHECK_INPUT_ARGUMENT(Kd.size() == 6,
87			"The size of the Kd vector needs to equal 6");
88			m_Kd = Kd;
89			}
90
91			void TaskSE3Equality::setReference(TrajectorySample& ref) {
92			m_ref = ref;
93			TSID_DISABLE_WARNING_PUSH
94			TSID_DISABLE_WARNING_DEPRECATED
95			PINOCCHIO_CHECK_INPUT_ARGUMENT(
96			ref.pos.size() == 12, "The size of the reference vector needs to be 12");
97			m_M_ref.translation(ref.pos.head<3>());
98			m_M_ref.rotation(MapMatrix3(&ref.pos(3), 3, 3));
99			TSID_DISABLE_WARNING_POP
100			m_v_ref = Motion(ref.getDerivative());
101			m_a_ref = Motion(ref.getSecondDerivative());
102			}
103
104			void TaskSE3Equality::setReference(const SE3& ref) {
105			TrajectorySample s(12, 6);
106			TSID_DISABLE_WARNING_PUSH
107			TSID_DISABLE_WARNING_DEPRECATED
108			tsid::math::SE3ToVector(ref, s.pos);
109			TSID_DISABLE_WARNING_POP
110			setReference(s);
111			}
112
113			const TrajectorySample& TaskSE3Equality::getReference() const { return m_ref; }
114
115			const Vector& TaskSE3Equality::position_error() const {
116			return m_p_error_masked_vec;
117			}
118
119			const Vector& TaskSE3Equality::velocity_error() const {
120			return m_v_error_masked_vec;
121			}
122
123			const Vector& TaskSE3Equality::position() const { return m_p; }
124
125			const Vector& TaskSE3Equality::velocity() const { return m_v; }
126
127			const Vector& TaskSE3Equality::position_ref() const { return m_p_ref; }
128
129			const Vector& TaskSE3Equality::velocity_ref() const { return m_v_ref_vec; }
130
131			const Vector& TaskSE3Equality::getDesiredAcceleration() const {
132			return m_a_des_masked;
133			}
134
135			Vector TaskSE3Equality::getAcceleration(ConstRefVector dv) const {
136			return m_constraint.matrix() * dv + m_drift_masked;
137			}
138
139			Index TaskSE3Equality::frame_id() const { return m_frame_id; }
140
141			const ConstraintBase& TaskSE3Equality::getConstraint() const {
142			return m_constraint;
143			}
144
145			void TaskSE3Equality::useLocalFrame(bool local_frame) {
146			m_local_frame = local_frame;
147			}
148
149			const ConstraintBase& TaskSE3Equality::compute(const double, ConstRefVector,
150			ConstRefVector, Data& data) {
151			SE3 oMi;
152			Motion v_frame;
153			m_robot.framePosition(data, m_frame_id, oMi);
154			m_robot.frameVelocity(data, m_frame_id, v_frame);
155			m_robot.frameClassicAcceleration(data, m_frame_id, m_drift);
156
157			// @todo Since Jacobian computation is cheaper in world frame
158			// we could do all computations in world frame
159			m_robot.frameJacobianLocal(data, m_frame_id, m_J);
160
161			errorInSE3(oMi, m_M_ref, m_p_error); // pos err in local frame
162			SE3ToVector(m_M_ref, m_p_ref);
163			SE3ToVector(oMi, m_p);
164
165			// Transformation from local to world
166			m_wMl.rotation(oMi.rotation());
167
168			if (m_local_frame) {
169			m_p_error_vec = m_p_error.toVector();
170			m_v_error = m_wMl.actInv(m_v_ref) - v_frame; // vel err in local frame
171
172			// desired acc in local frame
173			m_a_des = m_Kp.cwiseProduct(m_p_error_vec) +
174			m_Kd.cwiseProduct(m_v_error.toVector()) +
175			m_wMl.actInv(m_a_ref).toVector();
176			} else {
177			m_p_error_vec =
178			m_wMl.toActionMatrix() * // pos err in local world-oriented frame
179			m_p_error.toVector();
180
181			// cout<<"m_p_error_vec="<<m_p_error_vec.head<3>().transpose()<<endl;
182			// cout<<"oMi-m_M_ref
183			// ="<<-(oMi.translation()-m_M_ref.translation()).transpose()<<endl;
184
185			m_v_error =
186			m_v_ref - m_wMl.act(v_frame); // vel err in local world-oriented frame
187
188			m_drift = m_wMl.act(m_drift);
189
190			// desired acc in local world-oriented frame
191			m_a_des = m_Kp.cwiseProduct(m_p_error_vec) +
192			m_Kd.cwiseProduct(m_v_error.toVector()) + m_a_ref.toVector();
193
194			// Use an explicit temporary `m_J_rotated` here to avoid allocations.
195			m_J_rotated.noalias() = m_wMl.toActionMatrix() * m_J;
196			m_J = m_J_rotated;
197			}
198
199			m_v_error_vec = m_v_error.toVector();
200			m_v_ref_vec = m_v_ref.toVector();
201			m_v = v_frame.toVector();
202
203			int idx = 0;
204			for (int i = 0; i < 6; i++) {
205			if (m_mask(i) != 1.) continue;
206
207			m_constraint.matrix().row(idx) = m_J.row(i);
208			m_constraint.vector().row(idx) = (m_a_des - m_drift.toVector()).row(i);
209			m_a_des_masked(idx) = m_a_des(i);
210			m_drift_masked(idx) = m_drift.toVector()(i);
211			m_p_error_masked_vec(idx) = m_p_error_vec(i);
212			m_v_error_masked_vec(idx) = m_v_error_vec(i);
213
214			idx += 1;
215			}
216
217			return m_constraint;
218			}
219			} // namespace tasks
220			} // namespace tsid