Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/contacts/contact-two-frame-positions.cpp	Lines:	0	87	0.0 %
Date:	2024-02-02 08:47:34	Branches:	0	140	0.0 %


//
// Copyright (c) 2023 MIPT
//
// 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/contacts/contact-two-frame-positions.hpp"

#include <pinocchio/spatial/skew.hpp>

using namespace tsid;
using namespace contacts;
using namespace math;
using namespace trajectories;
using namespace tasks;

ContactTwoFramePositions::ContactTwoFramePositions(
    const std::string& name, RobotWrapper& robot, const std::string& frameName1,
    const std::string& frameName2, const double minNormalForce,
    const double maxNormalForce)
    : ContactBase(name, robot),
      m_motionTask(
          name, robot, frameName1,
          frameName2),  // Actual motion task with type TaskTwoFramesEquality
      m_forceInequality(name, 3, 3),
      m_forceRegTask(name, 3, 3),
      m_fMin(minNormalForce),
      m_fMax(maxNormalForce) {
  m_weightForceRegTask << 1, 1, 1;
  m_forceGenMat.resize(3, 3);
  m_fRef = Vector3::Zero();
  m_contactPoints.resize(3, 1);
  m_contactPoints.setZero();
  updateForceGeneratorMatrix();
  updateForceInequalityConstraints();
  updateForceRegularizationTask();

  // This contact has forceGenMat as 3x3 identity matrix, so it can be used only
  // for emulating a ball joint between two frames The forces calculated will
  // have only linear part (rotation will be unconstrained) So we need to set
  // the appropriate mask for motion task (which can take into account rotation
  // but we don't need it)
  math::Vector motion_mask(6);
  motion_mask << 1., 1., 1., 0., 0., 0.;
  m_motionTask.setMask(motion_mask);
}

void ContactTwoFramePositions::updateForceInequalityConstraints() {
  Matrix B = Matrix::Identity(3, 3);  // Force "gluing" two frames together can
                                      // be arbitrary in sign/direction
  Vector lb = m_fMin * Vector::Ones(3);
  Vector ub = m_fMax * Vector::Ones(3);

  m_forceInequality.setMatrix(B);
  m_forceInequality.setLowerBound(lb);
  m_forceInequality.setUpperBound(ub);
}

double ContactTwoFramePositions::getNormalForce(ConstRefVector f) const {
  return 0.0;
}

const Matrix3x& ContactTwoFramePositions::getContactPoints() const {
  return m_contactPoints;
}

void ContactTwoFramePositions::setRegularizationTaskWeightVector(
    ConstRefVector& w) {
  m_weightForceRegTask = w;
  updateForceRegularizationTask();
}

void ContactTwoFramePositions::updateForceRegularizationTask() {
  typedef Eigen::Matrix<double, 3, 3> Matrix3;
  Matrix3 A = Matrix3::Zero();
  A.diagonal() = m_weightForceRegTask;
  m_forceRegTask.setMatrix(A);
  m_forceRegTask.setVector(A * m_fRef);
}

void ContactTwoFramePositions::updateForceGeneratorMatrix() {
  m_forceGenMat.setIdentity();
}

unsigned int ContactTwoFramePositions::n_motion() const {
  return m_motionTask.dim();
}
unsigned int ContactTwoFramePositions::n_force() const { return 3; }

const Vector& ContactTwoFramePositions::Kp() {
  m_Kp3 = m_motionTask.Kp().head<3>();
  return m_Kp3;
}

const Vector& ContactTwoFramePositions::Kd() {
  m_Kd3 = m_motionTask.Kd().head<3>();
  return m_Kd3;
}

void ContactTwoFramePositions::Kp(ConstRefVector Kp) {
  assert(Kp.size() == 3);
  Vector6 Kp6;
  Kp6.head<3>() = Kp;
  m_motionTask.Kp(Kp6);
}

void ContactTwoFramePositions::Kd(ConstRefVector Kd) {
  assert(Kd.size() == 3);
  Vector6 Kd6;
  Kd6.head<3>() = Kd;
  m_motionTask.Kd(Kd6);
}

bool ContactTwoFramePositions::setContactNormal(ConstRefVector contactNormal) {
  return true;
}

bool ContactTwoFramePositions::setFrictionCoefficient(
    const double frictionCoefficient) {
  return true;
}

bool ContactTwoFramePositions::setMinNormalForce(const double minNormalForce) {
  m_fMin = minNormalForce;
  updateForceInequalityConstraints();
  return true;
}

bool ContactTwoFramePositions::setMaxNormalForce(const double maxNormalForce) {
  m_fMax = maxNormalForce;
  updateForceInequalityConstraints();
  return true;
}

void ContactTwoFramePositions::setForceReference(ConstRefVector& f_ref) {
  m_fRef = f_ref;
  updateForceRegularizationTask();
}

const ConstraintBase& ContactTwoFramePositions::computeMotionTask(
    const double t, ConstRefVector q, ConstRefVector v, Data& data) {
  return m_motionTask.compute(t, q, v, data);
}

const ConstraintInequality& ContactTwoFramePositions::computeForceTask(
    const double, ConstRefVector, ConstRefVector, const Data&) {
  return m_forceInequality;
}

const Matrix& ContactTwoFramePositions::getForceGeneratorMatrix() {
  return m_forceGenMat;
}

const ConstraintEquality&
ContactTwoFramePositions::computeForceRegularizationTask(const double,
                                                         ConstRefVector,
                                                         ConstRefVector,
                                                         const Data&) {
  return m_forceRegTask;
}

double ContactTwoFramePositions::getMinNormalForce() const { return m_fMin; }
double ContactTwoFramePositions::getMaxNormalForce() const { return m_fMax; }

const TaskTwoFramesEquality& ContactTwoFramePositions::getMotionTask() const {
  return m_motionTask;
}

const ConstraintBase& ContactTwoFramePositions::getMotionConstraint() const {
  return m_motionTask.getConstraint();
}

const ConstraintInequality& ContactTwoFramePositions::getForceConstraint()
    const {
  return m_forceInequality;
}

const ConstraintEquality& ContactTwoFramePositions::getForceRegularizationTask()
    const {
  return m_forceRegTask;
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			//
2			// Copyright (c) 2023 MIPT
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/contacts/contact-two-frame-positions.hpp"
20
21			#include <pinocchio/spatial/skew.hpp>
22
23			using namespace tsid;
24			using namespace contacts;
25			using namespace math;
26			using namespace trajectories;
27			using namespace tasks;
28
29			ContactTwoFramePositions::ContactTwoFramePositions(
30			const std::string& name, RobotWrapper& robot, const std::string& frameName1,
31			const std::string& frameName2, const double minNormalForce,
32			const double maxNormalForce)
33			: ContactBase(name, robot),
34			m_motionTask(
35			name, robot, frameName1,
36			frameName2), // Actual motion task with type TaskTwoFramesEquality
37			m_forceInequality(name, 3, 3),
38			m_forceRegTask(name, 3, 3),
39			m_fMin(minNormalForce),
40			m_fMax(maxNormalForce) {
41			m_weightForceRegTask << 1, 1, 1;
42			m_forceGenMat.resize(3, 3);
43			m_fRef = Vector3::Zero();
44			m_contactPoints.resize(3, 1);
45			m_contactPoints.setZero();
46			updateForceGeneratorMatrix();
47			updateForceInequalityConstraints();
48			updateForceRegularizationTask();
49
50			// This contact has forceGenMat as 3x3 identity matrix, so it can be used only
51			// for emulating a ball joint between two frames The forces calculated will
52			// have only linear part (rotation will be unconstrained) So we need to set
53			// the appropriate mask for motion task (which can take into account rotation
54			// but we don't need it)
55			math::Vector motion_mask(6);
56			motion_mask << 1., 1., 1., 0., 0., 0.;
57			m_motionTask.setMask(motion_mask);
58			}
59
60			void ContactTwoFramePositions::updateForceInequalityConstraints() {
61			Matrix B = Matrix::Identity(3, 3); // Force "gluing" two frames together can
62			// be arbitrary in sign/direction
63			Vector lb = m_fMin * Vector::Ones(3);
64			Vector ub = m_fMax * Vector::Ones(3);
65
66			m_forceInequality.setMatrix(B);
67			m_forceInequality.setLowerBound(lb);
68			m_forceInequality.setUpperBound(ub);
69			}
70
71			double ContactTwoFramePositions::getNormalForce(ConstRefVector f) const {
72			return 0.0;
73			}
74
75			const Matrix3x& ContactTwoFramePositions::getContactPoints() const {
76			return m_contactPoints;
77			}
78
79			void ContactTwoFramePositions::setRegularizationTaskWeightVector(
80			ConstRefVector& w) {
81			m_weightForceRegTask = w;
82			updateForceRegularizationTask();
83			}
84
85			void ContactTwoFramePositions::updateForceRegularizationTask() {
86			typedef Eigen::Matrix<double, 3, 3> Matrix3;
87			Matrix3 A = Matrix3::Zero();
88			A.diagonal() = m_weightForceRegTask;
89			m_forceRegTask.setMatrix(A);
90			m_forceRegTask.setVector(A * m_fRef);
91			}
92
93			void ContactTwoFramePositions::updateForceGeneratorMatrix() {
94			m_forceGenMat.setIdentity();
95			}
96
97			unsigned int ContactTwoFramePositions::n_motion() const {
98			return m_motionTask.dim();
99			}
100			unsigned int ContactTwoFramePositions::n_force() const { return 3; }
101
102			const Vector& ContactTwoFramePositions::Kp() {
103			m_Kp3 = m_motionTask.Kp().head<3>();
104			return m_Kp3;
105			}
106
107			const Vector& ContactTwoFramePositions::Kd() {
108			m_Kd3 = m_motionTask.Kd().head<3>();
109			return m_Kd3;
110			}
111
112			void ContactTwoFramePositions::Kp(ConstRefVector Kp) {
113			assert(Kp.size() == 3);
114			Vector6 Kp6;
115			Kp6.head<3>() = Kp;
116			m_motionTask.Kp(Kp6);
117			}
118
119			void ContactTwoFramePositions::Kd(ConstRefVector Kd) {
120			assert(Kd.size() == 3);
121			Vector6 Kd6;
122			Kd6.head<3>() = Kd;
123			m_motionTask.Kd(Kd6);
124			}
125
126			bool ContactTwoFramePositions::setContactNormal(ConstRefVector contactNormal) {
127			return true;
128			}
129
130			bool ContactTwoFramePositions::setFrictionCoefficient(
131			const double frictionCoefficient) {
132			return true;
133			}
134
135			bool ContactTwoFramePositions::setMinNormalForce(const double minNormalForce) {
136			m_fMin = minNormalForce;
137			updateForceInequalityConstraints();
138			return true;
139			}
140
141			bool ContactTwoFramePositions::setMaxNormalForce(const double maxNormalForce) {
142			m_fMax = maxNormalForce;
143			updateForceInequalityConstraints();
144			return true;
145			}
146
147			void ContactTwoFramePositions::setForceReference(ConstRefVector& f_ref) {
148			m_fRef = f_ref;
149			updateForceRegularizationTask();
150			}
151
152			const ConstraintBase& ContactTwoFramePositions::computeMotionTask(
153			const double t, ConstRefVector q, ConstRefVector v, Data& data) {
154			return m_motionTask.compute(t, q, v, data);
155			}
156
157			const ConstraintInequality& ContactTwoFramePositions::computeForceTask(
158			const double, ConstRefVector, ConstRefVector, const Data&) {
159			return m_forceInequality;
160			}
161
162			const Matrix& ContactTwoFramePositions::getForceGeneratorMatrix() {
163			return m_forceGenMat;
164			}
165
166			const ConstraintEquality&
167			ContactTwoFramePositions::computeForceRegularizationTask(const double,
168			ConstRefVector,
169			ConstRefVector,
170			const Data&) {
171			return m_forceRegTask;
172			}
173
174			double ContactTwoFramePositions::getMinNormalForce() const { return m_fMin; }
175			double ContactTwoFramePositions::getMaxNormalForce() const { return m_fMax; }
176
177			const TaskTwoFramesEquality& ContactTwoFramePositions::getMotionTask() const {
178			return m_motionTask;
179			}
180
181			const ConstraintBase& ContactTwoFramePositions::getMotionConstraint() const {
182			return m_motionTask.getConstraint();
183			}
184
185			const ConstraintInequality& ContactTwoFramePositions::getForceConstraint()
186			const {
187			return m_forceInequality;
188			}
189
190			const ConstraintEquality& ContactTwoFramePositions::getForceRegularizationTask()
191			const {
192			return m_forceRegTask;
193			}