Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/tools.cc	Lines:	4	162	2.5 %
Date:	2024-02-02 12:21:48	Branches:	3	234	1.3 %


// Copyright (c) 2014, LAAS-CNRS
// Authors: Steve Tonneau (steve.tonneau@laas.fr)
//
// This file is part of hpp-rbprm.
// hpp-rbprm 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.
//
// hpp-rbprm 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
// hpp-rbprm. If not, see <http://www.gnu.org/licenses/>.

#include <Eigen/Geometry>
#include <fstream>
#include <hpp/constraints/locked-joint.hh>
#include <hpp/core/config-projector.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/pinocchio/liegroup-element.hh>
#include <hpp/pinocchio/liegroup.hh>
#include <hpp/rbprm/tools.hh>
#include <iostream>

namespace hpp {
namespace tools {
Eigen::Matrix3d GetRotationMatrix(const Eigen::Vector3d& from,
                                  const Eigen::Vector3d& to) {
  Eigen::Quaterniond quat;
  quat.setFromTwoVectors(from, to);
  return quat.toRotationMatrix();
}

fcl::Matrix3f GetZRotMatrix(const core::value_type theta) {
  core::value_type c = cos(theta), s = sin(theta);
  fcl::Matrix3f r;
  r(0, 0) = c;
  r(0, 1) = -s;
  r(1, 0) = s;
  r(1, 1) = c;
  r(2, 2) = 1;
  return r;
}

fcl::Matrix3f GetYRotMatrix(const core::value_type theta) {
  core::value_type c = cos(theta), s = sin(theta);
  fcl::Matrix3f r;
  r(0, 0) = c;
  r(0, 2) = s;
  r(1, 1) = 1;
  r(2, 0) = -s;
  r(2, 2) = c;
  return r;
}

fcl::Matrix3f GetXRotMatrix(const core::value_type theta) {
  core::value_type c = cos(theta), s = sin(theta);
  fcl::Matrix3f r;
  r(0, 0) = 1;
  r(1, 1) = c;
  r(1, 2) = -s;
  r(2, 0) = s;
  r(2, 2) = c;
  return r;
}

pinocchio::value_type angleBetweenQuaternions(pinocchio::ConfigurationIn_t q1,
                                              pinocchio::ConfigurationIn_t q2,
                                              bool& cosIsNegative) {
  if (q1 == q2) {
    cosIsNegative = false;
    return 0;
  }
  pinocchio::value_type innerprod = q1.dot(q2);
  assert(fabs(innerprod) < 1.001);
  if (innerprod < -1) innerprod = -1;
  if (innerprod > 1) innerprod = 1;
  cosIsNegative = (innerprod < 0);
  pinocchio::value_type theta = acos(innerprod);
  return theta;
}

pinocchio::Configuration_t interpolate(pinocchio::ConfigurationIn_t q1,
                                       pinocchio::ConfigurationIn_t q2,
                                       const pinocchio::value_type& u) {
  bool cosIsNegative;
  pinocchio::value_type angle = angleBetweenQuaternions(q1, q2, cosIsNegative);
  const int invertor = (cosIsNegative) ? -1 : 1;
  const pinocchio::value_type theta = (cosIsNegative) ? (M_PI - angle) : angle;
  // theta is between 0 and M_PI/2.
  // sin(alpha*theta)/sin(theta) in 0 should be computed differently.
  if (fabs(theta) > 1e-4) {
    const pinocchio::value_type sintheta_inv = 1 / sin(theta);
    return (sin((1 - u) * theta) * sintheta_inv) * q1 +
           invertor * (sin(u * theta) * sintheta_inv) * q2;
  } else {
    return (1 - u) * q1 + invertor * u * q2;
  }
}

pinocchio::value_type distance(pinocchio::ConfigurationIn_t q1,
                               pinocchio::ConfigurationIn_t q2) {
  bool cosIsNegative;
  pinocchio::value_type theta = angleBetweenQuaternions(q1, q2, cosIsNegative);
  return 2 * (cosIsNegative ? M_PI - theta : theta);
}

void LockJoint(const pinocchio::JointPtr_t joint,
               hpp::core::ConfigProjectorPtr_t projector, const bool constant) {
  const core::Configuration_t& c = joint->robot()->currentConfiguration();
  core::size_type rankInConfiguration(joint->rankInConfiguration());
  core::LockedJointPtr_t lockedJoint = core::LockedJoint::create(
      joint, hpp::core::LiegroupElement(
                 c.segment(rankInConfiguration, joint->configSize()),
                 joint->configurationSpace()));
  if (!constant) {
    constraints::ComparisonTypes_t comps;
    comps.push_back(constraints::Equality);
    lockedJoint->comparisonType(comps);
  }
  projector->add(lockedJoint);
}

void LockJointRec(const std::string& spared, const pinocchio::JointPtr_t joint,
                  hpp::core::ConfigProjectorPtr_t projector) {
  if (joint->name() == spared) return;
  const core::Configuration_t& c = joint->robot()->currentConfiguration();
  core::size_type rankInConfiguration(joint->rankInConfiguration());
  projector->add(core::LockedJoint::create(
      joint, hpp::core::LiegroupElement(
                 c.segment(rankInConfiguration, joint->configSize()),
                 joint->configurationSpace())));
  for (std::size_t i = 0; i < joint->numberChildJoints(); ++i) {
    LockJointRec(spared, joint->childJoint(i), projector);
  }
}

void LockJointRec(const std::vector<std::string>& spared,
                  const pinocchio::JointPtr_t joint,
                  hpp::core::ConfigProjectorPtr_t projector) {
  if (std::find(spared.begin(), spared.end(), joint->name()) != spared.end())
    return;
  const core::Configuration_t& c = joint->robot()->currentConfiguration();
  core::size_type rankInConfiguration(joint->rankInConfiguration());
  projector->add(core::LockedJoint::create(
      joint, hpp::core::LiegroupElement(
                 c.segment(rankInConfiguration, joint->configSize()),
                 joint->configurationSpace())));
  for (std::size_t i = 0; i < joint->numberChildJoints(); ++i) {
    LockJointRec(spared, joint->childJoint(i), projector);
  }
}

namespace io {
double StrToD(const std::string& str) {
  std::istringstream ss(str);
  double result;
  return ss >> result ? result : 0;
}

int StrToI(const std::string& str) {
  std::istringstream ss(str);
  double result;
  return ss >> result ? (int)result : 0;
}

double StrToD(std::ifstream& input) {
  std::string line;
  getline(input, line);
  return StrToD(line);
}

int StrToI(std::ifstream& input) {
  std::string line;
  getline(input, line);
  return StrToI(line);
}

std::vector<std::string> splitString(const std::string& s, const char sep) {
  // Eclate une chane au niveau de ses ;.
  std::vector<std::string> ret;
  std::string s1 = "";
  for (unsigned int i = 0; i < s.size(); i++) {
    if (s[i] == sep || i == s.size() - 1) {
      if (i == s.size() - 1) s1 += s[i];
      if (s1 != "") ret.push_back(s1);
      s1 = "";
    } else
      s1 += s[i];
  }
  return ret;
}

void writeMatrix(const Eigen::MatrixXd& mat, std::ostream& output) {
  output << mat.rows() << ";" << mat.cols() << std::endl;
  for (int i = 0; i < mat.rows(); ++i) {
    for (int j = 0; j < mat.cols(); ++j) {
      output << mat(i, j) << ";";
    }
  }
}

Eigen::MatrixXd readMatrix(std::ifstream& myfile) {
  std::string line;
  return readMatrix(myfile, line);
}

Eigen::MatrixXd readMatrix(std::ifstream& myfile, std::string& line) {
  getline(myfile, line);  // id
  std::vector<std::string> dim = splitString(line, ';');
  int rows = StrToI(dim[0]);
  int cols = StrToI(dim[1]);
  Eigen::MatrixXd res(rows, cols);
  getline(myfile, line);  // id
  std::vector<std::string> data = splitString(line, ';');
  std::vector<std::string>::const_iterator current = data.begin();
  for (int i = 0; i < rows; ++i) {
    for (int j = 0; j < cols; ++j) {
      res(i, j) = StrToD(*current);
      ++current;
    }
  }
  return res;
}

void writeVecFCL(const fcl::Vec3f& vec, std::ostream& output) {
  for (int i = 0; i < 3; ++i) {
    output << vec[i] << ";";
  }
}

fcl::Vec3f readVecFCL(std::ifstream& myfile) {
  std::string line;
  return readVecFCL(myfile, line);
}

fcl::Vec3f readVecFCL(std::ifstream& myfile, std::string& line) {
  fcl::Vec3f res;
  getline(myfile, line);  // id
  std::vector<std::string> dim = splitString(line, ';');
  for (int i = 0; i < 3; ++i) {
    res[i] = StrToD(dim[i]);
  }
  return res;
}

void writeRotMatrixFCL(const fcl::Matrix3f& mat, std::ostream& output) {
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      output << mat(i, j) << ";";
    }
  }
}

fcl::Matrix3f readRotMatrixFCL(std::ifstream& myfile) {
  std::string line;
  return readRotMatrixFCL(myfile, line);
}

fcl::Matrix3f readRotMatrixFCL(std::ifstream& myfile, std::string& line) {
  fcl::Matrix3f res;
  getline(myfile, line);  // id
  std::vector<std::string> data = splitString(line, ';');
  std::vector<std::string>::const_iterator current = data.begin();
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      res(i, j) = StrToD(*current);
      ++current;
    }
  }
  return res;
}

}  // namespace io
}  // namespace tools
}  // namespace hpp


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			// Copyright (c) 2014, LAAS-CNRS
2			// Authors: Steve Tonneau (steve.tonneau@laas.fr)
3			//
4			// This file is part of hpp-rbprm.
5			// hpp-rbprm 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			//
10			// hpp-rbprm is distributed in the hope that it will be
11			// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
12			// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13			// General Lesser Public License for more details. You should have
14			// received a copy of the GNU Lesser General Public License along with
15			// hpp-rbprm. If not, see <http://www.gnu.org/licenses/>.
16
17			#include <Eigen/Geometry>
18			#include <fstream>
19			#include <hpp/constraints/locked-joint.hh>
20			#include <hpp/core/config-projector.hh>
21			#include <hpp/pinocchio/device.hh>
22			#include <hpp/pinocchio/joint.hh>
23			#include <hpp/pinocchio/liegroup-element.hh>
24			#include <hpp/pinocchio/liegroup.hh>
25			#include <hpp/rbprm/tools.hh>
26			#include <iostream>
27
28			namespace hpp {
29			namespace tools {
30		64	Eigen::Matrix3d GetRotationMatrix(const Eigen::Vector3d& from,
31			const Eigen::Vector3d& to) {
32	✓✗	64	Eigen::Quaterniond quat;
33	✓✗	64	quat.setFromTwoVectors(from, to);
34	✓✗	128	return quat.toRotationMatrix();
35			}
36
37			fcl::Matrix3f GetZRotMatrix(const core::value_type theta) {
38			core::value_type c = cos(theta), s = sin(theta);
39			fcl::Matrix3f r;
40			r(0, 0) = c;
41			r(0, 1) = -s;
42			r(1, 0) = s;
43			r(1, 1) = c;
44			r(2, 2) = 1;
45			return r;
46			}
47
48			fcl::Matrix3f GetYRotMatrix(const core::value_type theta) {
49			core::value_type c = cos(theta), s = sin(theta);
50			fcl::Matrix3f r;
51			r(0, 0) = c;
52			r(0, 2) = s;
53			r(1, 1) = 1;
54			r(2, 0) = -s;
55			r(2, 2) = c;
56			return r;
57			}
58
59			fcl::Matrix3f GetXRotMatrix(const core::value_type theta) {
60			core::value_type c = cos(theta), s = sin(theta);
61			fcl::Matrix3f r;
62			r(0, 0) = 1;
63			r(1, 1) = c;
64			r(1, 2) = -s;
65			r(2, 0) = s;
66			r(2, 2) = c;
67			return r;
68			}
69
70			pinocchio::value_type angleBetweenQuaternions(pinocchio::ConfigurationIn_t q1,
71			pinocchio::ConfigurationIn_t q2,
72			bool& cosIsNegative) {
73			if (q1 == q2) {
74			cosIsNegative = false;
75			return 0;
76			}
77			pinocchio::value_type innerprod = q1.dot(q2);
78			assert(fabs(innerprod) < 1.001);
79			if (innerprod < -1) innerprod = -1;
80			if (innerprod > 1) innerprod = 1;
81			cosIsNegative = (innerprod < 0);
82			pinocchio::value_type theta = acos(innerprod);
83			return theta;
84			}
85
86			pinocchio::Configuration_t interpolate(pinocchio::ConfigurationIn_t q1,
87			pinocchio::ConfigurationIn_t q2,
88			const pinocchio::value_type& u) {
89			bool cosIsNegative;
90			pinocchio::value_type angle = angleBetweenQuaternions(q1, q2, cosIsNegative);
91			const int invertor = (cosIsNegative) ? -1 : 1;
92			const pinocchio::value_type theta = (cosIsNegative) ? (M_PI - angle) : angle;
93			// theta is between 0 and M_PI/2.
94			// sin(alpha*theta)/sin(theta) in 0 should be computed differently.
95			if (fabs(theta) > 1e-4) {
96			const pinocchio::value_type sintheta_inv = 1 / sin(theta);
97			return (sin((1 - u) * theta) * sintheta_inv) * q1 +
98			invertor * (sin(u * theta) * sintheta_inv) * q2;
99			} else {
100			return (1 - u) * q1 + invertor * u * q2;
101			}
102			}
103
104			pinocchio::value_type distance(pinocchio::ConfigurationIn_t q1,
105			pinocchio::ConfigurationIn_t q2) {
106			bool cosIsNegative;
107			pinocchio::value_type theta = angleBetweenQuaternions(q1, q2, cosIsNegative);
108			return 2 * (cosIsNegative ? M_PI - theta : theta);
109			}
110
111			void LockJoint(const pinocchio::JointPtr_t joint,
112			hpp::core::ConfigProjectorPtr_t projector, const bool constant) {
113			const core::Configuration_t& c = joint->robot()->currentConfiguration();
114			core::size_type rankInConfiguration(joint->rankInConfiguration());
115			core::LockedJointPtr_t lockedJoint = core::LockedJoint::create(
116			joint, hpp::core::LiegroupElement(
117			c.segment(rankInConfiguration, joint->configSize()),
118			joint->configurationSpace()));
119			if (!constant) {
120			constraints::ComparisonTypes_t comps;
121			comps.push_back(constraints::Equality);
122			lockedJoint->comparisonType(comps);
123			}
124			projector->add(lockedJoint);
125			}
126
127			void LockJointRec(const std::string& spared, const pinocchio::JointPtr_t joint,
128			hpp::core::ConfigProjectorPtr_t projector) {
129			if (joint->name() == spared) return;
130			const core::Configuration_t& c = joint->robot()->currentConfiguration();
131			core::size_type rankInConfiguration(joint->rankInConfiguration());
132			projector->add(core::LockedJoint::create(
133			joint, hpp::core::LiegroupElement(
134			c.segment(rankInConfiguration, joint->configSize()),
135			joint->configurationSpace())));
136			for (std::size_t i = 0; i < joint->numberChildJoints(); ++i) {
137			LockJointRec(spared, joint->childJoint(i), projector);
138			}
139			}
140
141			void LockJointRec(const std::vector<std::string>& spared,
142			const pinocchio::JointPtr_t joint,
143			hpp::core::ConfigProjectorPtr_t projector) {
144			if (std::find(spared.begin(), spared.end(), joint->name()) != spared.end())
145			return;
146			const core::Configuration_t& c = joint->robot()->currentConfiguration();
147			core::size_type rankInConfiguration(joint->rankInConfiguration());
148			projector->add(core::LockedJoint::create(
149			joint, hpp::core::LiegroupElement(
150			c.segment(rankInConfiguration, joint->configSize()),
151			joint->configurationSpace())));
152			for (std::size_t i = 0; i < joint->numberChildJoints(); ++i) {
153			LockJointRec(spared, joint->childJoint(i), projector);
154			}
155			}
156
157			namespace io {
158			double StrToD(const std::string& str) {
159			std::istringstream ss(str);
160			double result;
161			return ss >> result ? result : 0;
162			}
163
164			int StrToI(const std::string& str) {
165			std::istringstream ss(str);
166			double result;
167			return ss >> result ? (int)result : 0;
168			}
169
170			double StrToD(std::ifstream& input) {
171			std::string line;
172			getline(input, line);
173			return StrToD(line);
174			}
175
176			int StrToI(std::ifstream& input) {
177			std::string line;
178			getline(input, line);
179			return StrToI(line);
180			}
181
182			std::vector<std::string> splitString(const std::string& s, const char sep) {
183			// Eclate une chane au niveau de ses ;.
184			std::vector<std::string> ret;
185			std::string s1 = "";
186			for (unsigned int i = 0; i < s.size(); i++) {
187			if (s[i] == sep \|\| i == s.size() - 1) {
188			if (i == s.size() - 1) s1 += s[i];
189			if (s1 != "") ret.push_back(s1);
190			s1 = "";
191			} else
192			s1 += s[i];
193			}
194			return ret;
195			}
196
197			void writeMatrix(const Eigen::MatrixXd& mat, std::ostream& output) {
198			output << mat.rows() << ";" << mat.cols() << std::endl;
199			for (int i = 0; i < mat.rows(); ++i) {
200			for (int j = 0; j < mat.cols(); ++j) {
201			output << mat(i, j) << ";";
202			}
203			}
204			}
205
206			Eigen::MatrixXd readMatrix(std::ifstream& myfile) {
207			std::string line;
208			return readMatrix(myfile, line);
209			}
210
211			Eigen::MatrixXd readMatrix(std::ifstream& myfile, std::string& line) {
212			getline(myfile, line); // id
213			std::vector<std::string> dim = splitString(line, ';');
214			int rows = StrToI(dim[0]);
215			int cols = StrToI(dim[1]);
216			Eigen::MatrixXd res(rows, cols);
217			getline(myfile, line); // id
218			std::vector<std::string> data = splitString(line, ';');
219			std::vector<std::string>::const_iterator current = data.begin();
220			for (int i = 0; i < rows; ++i) {
221			for (int j = 0; j < cols; ++j) {
222			res(i, j) = StrToD(*current);
223			++current;
224			}
225			}
226			return res;
227			}
228
229			void writeVecFCL(const fcl::Vec3f& vec, std::ostream& output) {
230			for (int i = 0; i < 3; ++i) {
231			output << vec[i] << ";";
232			}
233			}
234
235			fcl::Vec3f readVecFCL(std::ifstream& myfile) {
236			std::string line;
237			return readVecFCL(myfile, line);
238			}
239
240			fcl::Vec3f readVecFCL(std::ifstream& myfile, std::string& line) {
241			fcl::Vec3f res;
242			getline(myfile, line); // id
243			std::vector<std::string> dim = splitString(line, ';');
244			for (int i = 0; i < 3; ++i) {
245			res[i] = StrToD(dim[i]);
246			}
247			return res;
248			}
249
250			void writeRotMatrixFCL(const fcl::Matrix3f& mat, std::ostream& output) {
251			for (int i = 0; i < 3; ++i) {
252			for (int j = 0; j < 3; ++j) {
253			output << mat(i, j) << ";";
254			}
255			}
256			}
257
258			fcl::Matrix3f readRotMatrixFCL(std::ifstream& myfile) {
259			std::string line;
260			return readRotMatrixFCL(myfile, line);
261			}
262
263			fcl::Matrix3f readRotMatrixFCL(std::ifstream& myfile, std::string& line) {
264			fcl::Matrix3f res;
265			getline(myfile, line); // id
266			std::vector<std::string> data = splitString(line, ';');
267			std::vector<std::string>::const_iterator current = data.begin();
268			for (int i = 0; i < 3; ++i) {
269			for (int j = 0; j < 3; ++j) {
270			res(i, j) = StrToD(*current);
271			++current;
272			}
273			}
274			return res;
275			}
276
277			} // namespace io
278			} // namespace tools
279			} // namespace hpp