Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/rbprm-state.cc	Lines:	72	175	41.1 %
Date:	2024-02-02 12:21:48	Branches:	42	238	17.6 %


// 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 <hpp/rbprm/rbprm-state.hh>

namespace hpp {
namespace rbprm {

State::State(const State& other)
    : configuration_(other.configuration_),
      contactOrder_(other.contactOrder_),
      nbContacts(other.nbContacts),
      stable(other.stable),
      robustness(other.robustness) {
  contacts_ = (other.contacts_);
  contactNormals_ = (other.contactNormals_);
  contactPositions_ = (other.contactPositions_);
  contactRotation_ = (other.contactRotation_);
}

State& State::operator=(const State& other) {
  if (this != &other)  // protect against invalid self-assignment
  {
    contacts_ = other.contacts_;
    contactNormals_ = other.contactNormals_;
    contactPositions_ = other.contactPositions_;
    contactRotation_ = other.contactRotation_;
    contactOrder_ = other.contactOrder_;
    nbContacts = other.nbContacts;
    stable = other.stable;
    configuration_ = other.configuration_;
  }
  // by convention, always return *this
  return *this;
}

bool State::RemoveContact(const std::string& contactId) {
  if (contacts_.erase(contactId)) {
    contactNormals_.erase(contactId);
    contactPositions_.erase(contactId);
    contactRotation_.erase(contactId);
    contactNormals_.erase(contactId);
    --nbContacts;
    stable = false;
    std::queue<std::string> newQueue;
    std::string currentContact;
    while (!contactOrder_.empty()) {
      currentContact = contactOrder_.front();
      contactOrder_.pop();
      if (contactId != currentContact) {
        newQueue.push(currentContact);
      }
    }
    contactOrder_ = newQueue;
    return true;
  }
  return false;
}

std::string State::RemoveFirstContact() {
  if (contactOrder_.empty()) return "";
  std::string contactId = contactOrder_.front();
  contactOrder_.pop();
  contacts_.erase(contactId);
  contactNormals_.erase(contactId);
  contactPositions_.erase(contactId);
  contactRotation_.erase(contactId);
  contactNormals_.erase(contactId);
  stable = false;
  --nbContacts;
  return contactId;
}

void State::contactCreations(const State& previous,
                             std::vector<std::string>& outList) const {
  for (std::map<std::string, fcl::Vec3f>::const_iterator cit =
           contactPositions_.begin();
       cit != contactPositions_.end(); ++cit) {
    const std::string& name = cit->first;
    bool newContact(true);
    if (previous.contactPositions_.find(name) !=
        previous.contactPositions_.end()) {
      newContact =
          (previous.contactPositions_.at(name) - cit->second).norm() > 0.01;
    }
    if (newContact &&
        std::find(outList.begin(), outList.end(), name) == outList.end()) {
      outList.push_back(name);
    }
  }
}

void State::contactBreaks(const State& previous,
                          std::vector<std::string>& outList) const {
  previous.contactCreations(*this, outList);
}

std::vector<std::string> State::contactBreaks(const State& previous) const {
  std::vector<std::string> res;
  contactBreaks(previous, res);
  return res;
}

std::vector<std::string> State::contactCreations(const State& previous) const {
  std::vector<std::string> res;
  contactCreations(previous, res);
  return res;
}

namespace {
// Given known contact variations, computes all effector that were not in
// contacts
std::vector<std::string> freeLimbMotions(
    const std::vector<std::string>& allEffectors,
    const std::vector<std::string>& contactVariations, const State& current) {
  std::vector<std::string> res;
  for (std::vector<std::string>::const_iterator cit = allEffectors.begin();
       cit != allEffectors.end(); ++cit) {
    if (std::find(contactVariations.begin(), contactVariations.end(), *cit) ==
            contactVariations.end() &&
        !current.contacts_.at(*cit)) {
      res.push_back(*cit);
    }
  }
  return res;
}
}  // namespace

std::vector<std::string> State::freeVariations(
    const State& previous, const std::vector<std::string>& allEffectors) const {
  return freeLimbMotions(allEffectors, contactVariations(previous), *this);
}

std::vector<std::string> State::contactVariations(const State& previous) const {
  std::vector<std::string> res;
  contactCreations(previous, res);
  contactBreaks(previous, res);
  return res;
}

std::vector<std::string> State::fixedContacts(const State& previous) const {
  std::vector<std::string> res;
  std::vector<std::string> variations = contactVariations(previous);
  for (std::map<std::string, fcl::Vec3f>::const_iterator cit =
           contactPositions_.begin();
       cit != contactPositions_.end(); ++cit) {
    const std::string& name = cit->first;
    if (std::find(variations.begin(), variations.end(), name) ==
        variations.end()) {
      res.push_back(name);
    }
  }
  return res;
}

std::vector<std::string> State::allVariations(
    const State& previous, const std::vector<std::string>& allEffectors) const {
  std::vector<std::string> res;
  std::vector<std::string> fixedContacts = this->fixedContacts(previous);
  for (std::vector<std::string>::const_iterator cit = allEffectors.begin();
       cit != allEffectors.end(); ++cit) {
    if (std::find(fixedContacts.begin(), fixedContacts.end(), *cit) ==
        fixedContacts.end()) {
      res.push_back(*cit);
    }
  }
  return res;
}

void State::print() const {
  std::cout << " State " << std::endl;
  /*std::cout << " \t Configuration " << std::endl;
  for(int i = 0; i< configuration_.rows(); ++i)
  {
    std::cout << configuration_[i] << " ";
  }
  std::cout << std::endl;*/

  std::cout << " \t contacts " << std::endl;
  for (std::map<std::string, bool>::const_iterator cit = contacts_.begin();
       cit != contacts_.end(); ++cit) {
    std::cout << cit->first << ": " << cit->second << std::endl;
  }

  std::cout << "\t stable " << this->stable << std::endl;
  std::cout << "\t robustness " << this->robustness << std::endl;

  /*std::cout << " \t positions " << std::endl;
  for(std::map<std::string, fcl::Vec3f>::const_iterator cit =
    contactPositions_.begin(); cit != contactPositions_.end(); ++cit)
  {
    std::cout << cit->first << ": " <<  cit->second << std::endl;
  }*/
  /*std::cout << " \t contactNormals_ " << std::endl;
  for(std::map<std::string, fcl::Vec3f>::const_iterator cit =
    contactNormals_.begin(); cit != contactNormals_.end(); ++cit)
  {
    std::cout << cit->first << ": " <<  cit->second << std::endl;
  }
  std::cout << std::endl;*/
}

void State::printInternal(std::stringstream& ss) const {
  std::map<std::string, fcl::Vec3f>::const_iterator cit =
      contactNormals_.begin();
  for (unsigned int c = 0; c < nbContacts; ++c, ++cit) {
    const std::string& name = cit->first;
    const fcl::Vec3f& position = contactPositions_.at(name);
    const fcl::Matrix3f& rotation = contactRotation_.at(name);
    ss << name.substr(1);
    for (std::size_t i = 0; i < 3; ++i) {
      ss << " " << position[i];
    }
    for (std::size_t i = 0; i < 3; ++i) {
      for (std::size_t j = 0; j < 3; ++j) {
        ss << " " << rotation(i, j);
      }
    }
    ss << "\n";
  }
  ss << "configuration ";
  for (int i = 0; i < configuration_.rows(); ++i) {
    ss << " " << configuration_[i];
  }
  ss << "\n \n";
}

void State::print(std::stringstream& ss) const {
  ss << nbContacts << "\n";
  ss << "";
  std::map<std::string, fcl::Vec3f>::const_iterator cit =
      contactNormals_.begin();
  for (unsigned int c = 0; c < nbContacts; ++c, ++cit) {
    ss << " " << cit->first << " ";
  }
  ss << "\n";
  printInternal(ss);
}

void State::print(std::stringstream& ss, const State& previous) const {
  ss << nbContacts << "\n";
  std::vector<std::string> ncontacts;
  ss << "";
  for (std::map<std::string, fcl::Vec3f>::const_iterator cit =
           contactPositions_.begin();
       cit != contactPositions_.end(); ++cit) {
    const std::string& name = cit->first;
    bool newContact(true);
    if (previous.contactPositions_.find(name) !=
        previous.contactPositions_.end()) {
      newContact =
          (previous.contactPositions_.at(name) - cit->second).norm() > 0.01;
    }
    if (newContact) {
      ncontacts.push_back(name);
      ss << name.substr(1) << " ";
    }
  }
  ss << "\n";
  /*ss << "broken Contacts: ";
  for(std::map<std::string, fcl::Vec3f>::const_iterator cit =
  previous.contactPositions_.begin(); cit != previous.contactPositions_.end();
  ++cit)
  {
      const std::string& name = cit->first;
      if(contactPositions_.find(name) == contactPositions_.end())
      {
          ss << name << " ";
      }
  }
  ss << "\n";*/
  printInternal(ss);
  /*for(std::vector<std::string>::const_iterator cit = ncontacts.begin();
      cit != ncontacts.end(); ++cit)
  {
      const std::string& name = *cit;
      const fcl::Vec3f& normal = contactNormals_.at(name);
      const fcl::Vec3f& position = contactPositions_.at(name);
      ss << " " << name <<": ";
      for(std::size_t i=0; i<3; ++i)
      {
          ss << " " << position[i];
      }
      for(std::size_t i=0; i<3; ++i)
      {
          ss << " " << normal[i];
      }
      ss << "\n";
  }
  for(int i=0; i<configuration_.rows(); ++i)
  {
      ss << " " << configuration_[i];
  }
  ss << "\n \n";*/
}

pinocchio::value_type effectorDistance(const State& from, const State& to) {
  std::vector<std::string> variations = to.contactCreations(from);
  pinocchio::value_type norm = 0.;
  for (std::vector<std::string>::const_iterator cit = variations.begin();
       cit != variations.end(); ++cit) {
    std::string name = *cit;
    if (from.contactPositions_.find(name) != from.contactPositions_.end()) {
      norm = std::max(norm, (from.contactPositions_.at(name) -
                             to.contactPositions_.at(name))
                                .norm());
    }
  }
  return norm;
}

}  // namespace rbprm
}  // 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 <hpp/rbprm/rbprm-state.hh>
18
19			namespace hpp {
20			namespace rbprm {
21
22		16	State::State(const State& other)
23		16	: configuration_(other.configuration_),
24		16	contactOrder_(other.contactOrder_),
25		16	nbContacts(other.nbContacts),
26		16	stable(other.stable),
27	✓✗	16	robustness(other.robustness) {
28	✓✗	16	contacts_ = (other.contacts_);
29	✓✗	16	contactNormals_ = (other.contactNormals_);
30	✓✗	16	contactPositions_ = (other.contactPositions_);
31	✓✗	16	contactRotation_ = (other.contactRotation_);
32		16	}
33
34		235	State& State::operator=(const State& other) {
35	✓✗	235	if (this != &other) // protect against invalid self-assignment
36			{
37		235	contacts_ = other.contacts_;
38		235	contactNormals_ = other.contactNormals_;
39		235	contactPositions_ = other.contactPositions_;
40		235	contactRotation_ = other.contactRotation_;
41		235	contactOrder_ = other.contactOrder_;
42		235	nbContacts = other.nbContacts;
43		235	stable = other.stable;
44		235	configuration_ = other.configuration_;
45			}
46			// by convention, always return *this
47		235	return *this;
48			}
49
50		16	bool State::RemoveContact(const std::string& contactId) {
51	✓✗	16	if (contacts_.erase(contactId)) {
52	✓✗	16	contactNormals_.erase(contactId);
53	✓✗	16	contactPositions_.erase(contactId);
54	✓✗	16	contactRotation_.erase(contactId);
55	✓✗	16	contactNormals_.erase(contactId);
56		16	--nbContacts;
57		16	stable = false;
58	✓✗	32	std::queue<std::string> newQueue;
59		16	std::string currentContact;
60	✓✓	48	while (!contactOrder_.empty()) {
61	✓✗	32	currentContact = contactOrder_.front();
62		32	contactOrder_.pop();
63	✓✓	32	if (contactId != currentContact) {
64	✓✗	16	newQueue.push(currentContact);
65			}
66			}
67	✓✗	16	contactOrder_ = newQueue;
68		16	return true;
69			}
70			return false;
71			}
72
73			std::string State::RemoveFirstContact() {
74			if (contactOrder_.empty()) return "";
75			std::string contactId = contactOrder_.front();
76			contactOrder_.pop();
77			contacts_.erase(contactId);
78			contactNormals_.erase(contactId);
79			contactPositions_.erase(contactId);
80			contactRotation_.erase(contactId);
81			contactNormals_.erase(contactId);
82			stable = false;
83			--nbContacts;
84			return contactId;
85			}
86
87		268	void State::contactCreations(const State& previous,
88			std::vector<std::string>& outList) const {
89		944	for (std::map<std::string, fcl::Vec3f>::const_iterator cit =
90		268	contactPositions_.begin();
91	✓✓	2156	cit != contactPositions_.end(); ++cit) {
92		944	const std::string& name = cit->first;
93		944	bool newContact(true);
94	✓✗	944	if (previous.contactPositions_.find(name) !=
95	✓✗	1888	previous.contactPositions_.end()) {
96		944	newContact =
97	✓✗✓✗ ✓✗	944	(previous.contactPositions_.at(name) - cit->second).norm() > 0.01;
98			}
99	✓✓✓✓	1008	if (newContact &&
100	✓✗✓✓	1008	std::find(outList.begin(), outList.end(), name) == outList.end()) {
101	✓✗	48	outList.push_back(name);
102			}
103			}
104		268	}
105
106		134	void State::contactBreaks(const State& previous,
107			std::vector<std::string>& outList) const {
108		134	previous.contactCreations(*this, outList);
109		134	}
110
111			std::vector<std::string> State::contactBreaks(const State& previous) const {
112			std::vector<std::string> res;
113			contactBreaks(previous, res);
114			return res;
115			}
116
117			std::vector<std::string> State::contactCreations(const State& previous) const {
118			std::vector<std::string> res;
119			contactCreations(previous, res);
120			return res;
121			}
122
123			namespace {
124			// Given known contact variations, computes all effector that were not in
125			// contacts
126			std::vector<std::string> freeLimbMotions(
127			const std::vector<std::string>& allEffectors,
128			const std::vector<std::string>& contactVariations, const State& current) {
129			std::vector<std::string> res;
130			for (std::vector<std::string>::const_iterator cit = allEffectors.begin();
131			cit != allEffectors.end(); ++cit) {
132			if (std::find(contactVariations.begin(), contactVariations.end(), *cit) ==
133			contactVariations.end() &&
134			!current.contacts_.at(*cit)) {
135			res.push_back(*cit);
136			}
137			}
138			return res;
139			}
140			} // namespace
141
142			std::vector<std::string> State::freeVariations(
143			const State& previous, const std::vector<std::string>& allEffectors) const {
144			return freeLimbMotions(allEffectors, contactVariations(previous), *this);
145			}
146
147		118	std::vector<std::string> State::contactVariations(const State& previous) const {
148		118	std::vector<std::string> res;
149	✓✗	118	contactCreations(previous, res);
150	✓✗	118	contactBreaks(previous, res);
151		118	return res;
152			}
153
154		102	std::vector<std::string> State::fixedContacts(const State& previous) const {
155		102	std::vector<std::string> res;
156	✓✗	204	std::vector<std::string> variations = contactVariations(previous);
157		408	for (std::map<std::string, fcl::Vec3f>::const_iterator cit =
158		102	contactPositions_.begin();
159	✓✓	918	cit != contactPositions_.end(); ++cit) {
160		408	const std::string& name = cit->first;
161	✓✗	408	if (std::find(variations.begin(), variations.end(), name) ==
162	✓✗	816	variations.end()) {
163	✓✗	408	res.push_back(name);
164			}
165			}
166		204	return res;
167			}
168
169			std::vector<std::string> State::allVariations(
170			const State& previous, const std::vector<std::string>& allEffectors) const {
171			std::vector<std::string> res;
172			std::vector<std::string> fixedContacts = this->fixedContacts(previous);
173			for (std::vector<std::string>::const_iterator cit = allEffectors.begin();
174			cit != allEffectors.end(); ++cit) {
175			if (std::find(fixedContacts.begin(), fixedContacts.end(), *cit) ==
176			fixedContacts.end()) {
177			res.push_back(*cit);
178			}
179			}
180			return res;
181			}
182
183			void State::print() const {
184			std::cout << " State " << std::endl;
185			/*std::cout << " \t Configuration " << std::endl;
186			for(int i = 0; i< configuration_.rows(); ++i)
187			{
188			std::cout << configuration_[i] << " ";
189			}
190			std::cout << std::endl;*/
191
192			std::cout << " \t contacts " << std::endl;
193			for (std::map<std::string, bool>::const_iterator cit = contacts_.begin();
194			cit != contacts_.end(); ++cit) {
195			std::cout << cit->first << ": " << cit->second << std::endl;
196			}
197
198			std::cout << "\t stable " << this->stable << std::endl;
199			std::cout << "\t robustness " << this->robustness << std::endl;
200
201			/*std::cout << " \t positions " << std::endl;
202			for(std::map<std::string, fcl::Vec3f>::const_iterator cit =
203			contactPositions_.begin(); cit != contactPositions_.end(); ++cit)
204			{
205			std::cout << cit->first << ": " << cit->second << std::endl;
206			}*/
207			/*std::cout << " \t contactNormals_ " << std::endl;
208			for(std::map<std::string, fcl::Vec3f>::const_iterator cit =
209			contactNormals_.begin(); cit != contactNormals_.end(); ++cit)
210			{
211			std::cout << cit->first << ": " << cit->second << std::endl;
212			}
213			std::cout << std::endl;*/
214			}
215
216			void State::printInternal(std::stringstream& ss) const {
217			std::map<std::string, fcl::Vec3f>::const_iterator cit =
218			contactNormals_.begin();
219			for (unsigned int c = 0; c < nbContacts; ++c, ++cit) {
220			const std::string& name = cit->first;
221			const fcl::Vec3f& position = contactPositions_.at(name);
222			const fcl::Matrix3f& rotation = contactRotation_.at(name);
223			ss << name.substr(1);
224			for (std::size_t i = 0; i < 3; ++i) {
225			ss << " " << position[i];
226			}
227			for (std::size_t i = 0; i < 3; ++i) {
228			for (std::size_t j = 0; j < 3; ++j) {
229			ss << " " << rotation(i, j);
230			}
231			}
232			ss << "\n";
233			}
234			ss << "configuration ";
235			for (int i = 0; i < configuration_.rows(); ++i) {
236			ss << " " << configuration_[i];
237			}
238			ss << "\n \n";
239			}
240
241			void State::print(std::stringstream& ss) const {
242			ss << nbContacts << "\n";
243			ss << "";
244			std::map<std::string, fcl::Vec3f>::const_iterator cit =
245			contactNormals_.begin();
246			for (unsigned int c = 0; c < nbContacts; ++c, ++cit) {
247			ss << " " << cit->first << " ";
248			}
249			ss << "\n";
250			printInternal(ss);
251			}
252
253			void State::print(std::stringstream& ss, const State& previous) const {
254			ss << nbContacts << "\n";
255			std::vector<std::string> ncontacts;
256			ss << "";
257			for (std::map<std::string, fcl::Vec3f>::const_iterator cit =
258			contactPositions_.begin();
259			cit != contactPositions_.end(); ++cit) {
260			const std::string& name = cit->first;
261			bool newContact(true);
262			if (previous.contactPositions_.find(name) !=
263			previous.contactPositions_.end()) {
264			newContact =
265			(previous.contactPositions_.at(name) - cit->second).norm() > 0.01;
266			}
267			if (newContact) {
268			ncontacts.push_back(name);
269			ss << name.substr(1) << " ";
270			}
271			}
272			ss << "\n";
273			/*ss << "broken Contacts: ";
274			for(std::map<std::string, fcl::Vec3f>::const_iterator cit =
275			previous.contactPositions_.begin(); cit != previous.contactPositions_.end();
276			++cit)
277			{
278			const std::string& name = cit->first;
279			if(contactPositions_.find(name) == contactPositions_.end())
280			{
281			ss << name << " ";
282			}
283			}
284			ss << "\n";*/
285			printInternal(ss);
286			/*for(std::vector<std::string>::const_iterator cit = ncontacts.begin();
287			cit != ncontacts.end(); ++cit)
288			{
289			const std::string& name = *cit;
290			const fcl::Vec3f& normal = contactNormals_.at(name);
291			const fcl::Vec3f& position = contactPositions_.at(name);
292			ss << " " << name <<": ";
293			for(std::size_t i=0; i<3; ++i)
294			{
295			ss << " " << position[i];
296			}
297			for(std::size_t i=0; i<3; ++i)
298			{
299			ss << " " << normal[i];
300			}
301			ss << "\n";
302			}
303			for(int i=0; i<configuration_.rows(); ++i)
304			{
305			ss << " " << configuration_[i];
306			}
307			ss << "\n \n";*/
308			}
309
310			pinocchio::value_type effectorDistance(const State& from, const State& to) {
311			std::vector<std::string> variations = to.contactCreations(from);
312			pinocchio::value_type norm = 0.;
313			for (std::vector<std::string>::const_iterator cit = variations.begin();
314			cit != variations.end(); ++cit) {
315			std::string name = *cit;
316			if (from.contactPositions_.find(name) != from.contactPositions_.end()) {
317			norm = std::max(norm, (from.contactPositions_.at(name) -
318			to.contactPositions_.at(name))
319			.norm());
320			}
321			}
322			return norm;
323			}
324
325			} // namespace rbprm
326			} // namespace hpp