GCC Code Coverage Report

Line	Branch	Exec	Source
1			// Copyright (C) 2009, 2010 by Florent Lamiraux, Thomas Moulard, JRL.
2			//
3			// This file is part of the hpp-affordance-corba.
4			//
5			// This software is provided "as is" without warranty of any kind,
6			// either expressed or implied, including but not limited to the
7			// implied warranties of fitness for a particular purpose.
8			//
9			// See the COPYING file for more information.
10
11			#include "affordance.impl.hh"
12
13			#include <hpp/core/problem-solver.hh>
14			#include <hpp/pinocchio/collision-object.hh>
15			#include <hpp/util/debug.hh>
16			#include <iostream>
17			#include <pinocchio/fwd.hpp>
18			#include <string>
19
20			#include "hpp/affordance/affordance-extraction.hh"
21			#include "hpp/affordance/operations.hh"
22
23			namespace hpp {
24			typedef std::vector<pinocchio::CollisionObjectPtr_t> ObjectStdVector_t;
25			typedef hpp::core::FclCollisionObject FclCollisionObject;
26			typedef hpp::core::FclCollisionObject* FclCollisionObjectPtr_t;
27			typedef hpp::core::FclCollisionObjectSharePtr_t FclCollisionObjectSharePtr_t;
28			typedef hpp::core::AffordanceObjects_t AffordanceObjects_t;
29			namespace affordanceCorba {
30			namespace impl {
31
32			const std::string affSuffix = "aff";
33
34		✗	Afford::Afford() {}
35
36		✗	Afford::Afford(const core::ProblemSolverPtr_t& /*problemSolver*/) {}
37
38		✗	void Afford::resetAffordanceConfig() {
39		✗	problemSolver()->affordanceConfigs.add("Support", vector3_t(0.3, 0.3, 0.05));
40		✗	problemSolver()->affordanceConfigs.add("Lean", vector3_t(0.1, 0.3, 0.05));
41		✗	problemSolver()->affordanceConfigs.add("Support45",
42		✗	vector3_t(0.1, 0.3, 0.05));
43		✗	}
44
45		✗	affordance::OperationBases_t Afford::createOperations() {
46		✗	if (!problemSolver()->affordanceConfigs.has("Support")) {
47			throw hpp::Error(
48		✗	"No 'Support' affordance type found Afford::createOperations ()");
49			}
50			const hpp::pinocchio::vector3_t& sconf =
51		✗	problemSolver()->affordanceConfigs.get("Support");
52
53		✗	if (!problemSolver()->affordanceConfigs.has("Lean")) {
54			throw hpp::Error(
55		✗	"No 'Lean' affordance type found in Afford::createOperations ()");
56			}
57			const hpp::pinocchio::vector3_t& lconf =
58		✗	problemSolver()->affordanceConfigs.get("Lean");
59
60		✗	if (!problemSolver()->affordanceConfigs.has("Support45")) {
61			throw hpp::Error(
62		✗	"No 'Support45' affordance type found in Afford::createOperations ()");
63			}
64			const hpp::pinocchio::vector3_t& s45conf =
65		✗	problemSolver()->affordanceConfigs.get("Support45");
66
67			affordance::SupportOperationPtr_t support(
68		✗	new affordance::SupportOperation(sconf[0], sconf[1], sconf[2]));
69			affordance::LeanOperationPtr_t lean(
70		✗	new affordance::LeanOperation(lconf[0], lconf[1], lconf[2]));
71			affordance::Support45OperationPtr_t support45(
72		✗	new affordance::Support45Operation(s45conf[0], s45conf[1], s45conf[2]));
73
74		✗	affordance::OperationBases_t operations;
75		✗	operations.push_back(support);
76		✗	operations.push_back(lean);
77		✗	operations.push_back(support45);
78
79		✗	return operations;
80		✗	}
81
82		✗	void Afford::setAffordanceConfig(const char* affType,
83			const hpp::doubleSeq& conf) {
84		✗	if (conf.length() != 3) {
85		✗	throw hpp::Error("Configuration vector has invalid size.");
86			}
87		✗	const vector3_t config(conf[0], conf[1], conf[2]);
88		✗	problemSolver()->affordanceConfigs.add(affType, config);
89
90			/*const std::map<std::string, core::AffordanceConfig_t> map =
91			problemSolver()->map <core::AffordanceConfig_t> ();*/
92		✗	}
93		✗	hpp::doubleSeq* Afford::getAffordanceConfig(const char* affType) {
94		✗	if (!problemSolver()->affordanceConfigs.has(affType)) {
95			throw hpp::Error(
96		✗	"No given affordance type found in Afford::getAffordanceConfig");
97			}
98		✗	const vector3_t& config = problemSolver()->affordanceConfigs.get(affType);
99		✗	hpp::doubleSeq* conf = new hpp::doubleSeq();
100		✗	conf->length((CORBA::ULong)config.size());
101		✗	for (std::size_t idx = 0; idx < conf->length(); idx++) {
102		✗	(*conf)[(CORBA::ULong)idx] = config[idx];
103			}
104		✗	return conf;
105			}
106
107		✗	void Afford::setMargin(const char* affType, CORBA::Double margin) {
108		✗	if (!problemSolver()->affordanceConfigs.has(affType)) {
109		✗	throw hpp::Error("No given affordance type found in Afford::setMargin");
110			}
111		✗	vector3_t config = problemSolver()->affordanceConfigs.get(affType);
112		✗	config[0] = margin;
113
114		✗	problemSolver()->affordanceConfigs.add(affType, config);
115		✗	}
116
117		✗	void Afford::setNeighbouringTriangleMargin(const char* affType,
118			CORBA::Double nbTriMargin) {
119		✗	if (!problemSolver()->affordanceConfigs.has(affType)) {
120			throw hpp::Error(
121			"No given affordance type found in "
122		✗	"Afford::setNeighbouringTriangleMargin");
123			}
124		✗	vector3_t config = problemSolver()->affordanceConfigs.get(affType);
125		✗	config[1] = nbTriMargin;
126
127		✗	problemSolver()->affordanceConfigs.add(affType, config);
128		✗	}
129
130		✗	void Afford::setMinimumArea(const char* affType, CORBA::Double minArea) {
131		✗	if (!problemSolver()->affordanceConfigs.has(affType)) {
132			throw hpp::Error(
133		✗	"No given affordance type found in Afford::setMinimunArea");
134			}
135		✗	vector3_t config = problemSolver()->affordanceConfigs.get(affType);
136		✗	config[2] = minArea;
137
138		✗	problemSolver()->affordanceConfigs.add(affType, config);
139		✗	}
140
141		✗	bool isBVHModelTriangles(
142			const hpp::pinocchio::FclCollisionObjectPtr_t& object) {
143		✗	if (object->collisionGeometry()->getNodeType() == coal::BV_OBBRSS) {
144			const affordance::BVHModelOBConst_Ptr_t model =
145			static_pointer_cast<const affordance::BVHModelOB>(
146		✗	object->collisionGeometry());
147		✗	if (model->getModelType() == coal::BVH_MODEL_TRIANGLES) {
148		✗	return true;
149			}
150		✗	}
151		✗	return false;
152			}
153
154		✗	bool Afford::checkModel(const char* obstacleName) {
155			std::list<std::string> obstacles =
156		✗	problemSolver()->obstacleNames(false, true);
157			std::list<std::string>::iterator objIt =
158		✗	std::find(obstacles.begin(), obstacles.end(), obstacleName);
159		✗	if (objIt == obstacles.end()) {
160		✗	throw hpp::Error("No obstacle by given name found. Unable to analyse.");
161			}
162		✗	if (!isBVHModelTriangles((problemSolver()->obstacle(obstacleName))->fcl())) {
163		✗	return false; // wrong model type -> return false
164			}
165		✗	return true;
166		✗	}
167
168		✗	void Afford::affordanceAnalysis(const char* obstacleName,
169			const affordance::OperationBases_t& operations,
170			std::vector<double> reduceSizes) {
171		✗	std::list<std::string> obstacles = problemSolver()->obstacleNames(true, true);
172			std::list<std::string>::iterator objIt =
173		✗	std::find(obstacles.begin(), obstacles.end(), obstacleName);
174		✗	while (reduceSizes.size() < operations.size()) reduceSizes.push_back(0.);
175
176		✗	if (objIt == obstacles.end()) {
177		✗	throw hpp::Error("No obstacle by given name found. Unable to analyse.");
178			}
179			try {
180			affordance::SemanticsDataPtr_t aff = affordance::affordanceAnalysis(
181		✗	(problemSolver()->obstacle(obstacleName)->fcl()), operations);
182			std::vector<std::vector<coal::CollisionObjectPtr_t> > affObjs =
183		✗	affordance::getReducedAffordanceObjects(aff, reduceSizes);
184			// add coal::CollisionObstacles to problemSolver
185		✗	addAffObjects(operations, affObjs, obstacleName);
186		✗	} catch (const std::exception& exc) {
187		✗	throw Error(exc.what());
188		✗	}
189		✗	}
190
191		✗	void Afford::analyseObject(const char* obstacleName,
192			const hpp::doubleSeq& reduceSizesCorba) {
193		✗	std::vector<double> reduceSizes; // copy corba list to vector
194		✗	for (size_type i = 0; i < (size_type)reduceSizesCorba.length(); ++i) {
195		✗	reduceSizes.push_back(reduceSizesCorba[(CORBA::ULong)i]);
196			}
197			// first erase affordance information for obstacleName
198		✗	deleteAffordances(obstacleName);
199		✗	affordance::OperationBases_t operations = createOperations();
200		✗	affordanceAnalysis(obstacleName, operations, reduceSizes);
201		✗	}
202
203		✗	void Afford::analyseAll(const hpp::doubleSeq& reduceSizesCorba) {
204		✗	std::vector<double> reduceSizes; // copy corba list to vector
205		✗	for (size_type i = 0; i < (size_type)reduceSizesCorba.length(); ++i) {
206		✗	reduceSizes.push_back(reduceSizesCorba[(CORBA::ULong)i]);
207			}
208			// first clear all old affordances:
209		✗	problemSolver()
210			->affordanceObjects
211		✗	.clear(); // clear
212			// <std::vector<shared_ptr<hpp::pinocchio::CollisionObject>
213			// > > ();
214		✗	affordance::OperationBases_t operations = createOperations();
215		✗	for (hpp::ObjectStdVector_t::const_iterator objIt =
216		✗	problemSolver()->collisionObstacles().begin();
217		✗	objIt != problemSolver()->collisionObstacles().end(); objIt++) {
218		✗	const char* obstacleName = (*objIt)->name().c_str();
219		✗	affordanceAnalysis(obstacleName, operations, reduceSizes);
220			}
221		✗	}
222
223			// delete affordances by type for given object
224		✗	void Afford::deleteAffordancesByType(const char* affordance,
225			const char* obstacleName) {
226		✗	const std::string noObject = "";
227		✗	if (obstacleName == noObject) {
228		✗	problemSolver()->affordanceObjects.erase(affordance);
229			} else {
230		✗	if (!problemSolver()->affordanceObjects.has(affordance)) {
231			std::cout
232		✗	<< "Afford::deleteAffordanceByType: no affordance objects to delete"
233		✗	<< std::endl;
234		✗	return;
235			}
236			AffordanceObjects_t affs =
237		✗	problemSolver()->affordanceObjects.get(affordance);
238
239		✗	for (unsigned int objIdx = 0; objIdx < affs.size(); objIdx++) {
240		✗	if (affs[objIdx].first == obstacleName) {
241		✗	affs.erase(affs.begin() + objIdx);
242		✗	objIdx--;
243			}
244			}
245		✗	problemSolver()->affordanceObjects.add(affordance, affs);
246		✗	}
247		✗	}
248
249			// delete all affordances for given object
250		✗	void Afford::deleteAffordances(const char* obstacleNameNonAff) {
251		✗	std::string obstacleName(obstacleNameNonAff);
252		✗	obstacleName += affSuffix;
253		✗	const std::string noObject = "";
254		✗	if (obstacleName == noObject) {
255			// if no obstacleName given, delete all affs in problemSolver
256			// problemSolver()->clear
257			// <std::vector<shared_ptr<hpp::pinocchio::CollisionObject> > > ();
258		✗	problemSolver()->affordanceObjects.clear();
259			} else {
260			std::list<std::string> keys =
261		✗	problemSolver()->obstacleNames(true, false); /*problemSolver()->getKeys
262			<std::vector<shared_ptr<hpp::pinocchio::CollisionObject> >,
263			std::list<std::string> > ();*/
264		✗	std::list<std::string>::iterator affIt = keys.begin();
265		✗	for (; affIt != keys.end(); affIt++) {
266		✗	for (std::map<std::string, AffordanceObjects_t>::iterator kit =
267		✗	problemSolver()->affordanceObjects.map.begin();
268		✗	kit != problemSolver()->affordanceObjects.map.end(); ++kit) {
269		✗	if ((kit->first).find((*affIt)) != std::string::npos) {
270		✗	problemSolver()->removeObstacle(kit->first);
271		✗	AffordanceObjects_t& affs = kit->second;
272		✗	for (unsigned int objIdx = 0; objIdx < affs.size(); objIdx++) {
273		✗	if (affs[objIdx].first == obstacleName) {
274		✗	affs.erase(affs.begin() + objIdx);
275		✗	objIdx--;
276			}
277			}
278		✗	problemSolver()->affordanceObjects.add(*affIt, affs);
279			}
280			}
281			}
282		✗	}
283		✗	}
284
285		✗	void Afford::addAffObjects(
286			const affordance::OperationBases_t& ops,
287			const std::vector<affordance::CollisionObjects_t>& affObjs,
288			const char* obstacleNameNonAff) {
289		✗	std::string obstacleName(obstacleNameNonAff);
290		✗	obstacleName += affSuffix;
291		✗	for (unsigned int opIdx = 0; opIdx < ops.size(); opIdx++) {
292		✗	AffordanceObjects_t objs;
293		✗	affordance::CollisionObjects_t affs = affObjs[opIdx];
294		✗	for (unsigned int objIdx = 0; objIdx < affs.size(); objIdx++) {
295			/*FclCollisionObjectSharePtr_t obj =
296			FclCollisionObjectSharePtr_t (new FclCollisionObject(*(affs[objIdx])));*/
297		✗	std::stringstream ss;
298		✗	ss << opIdx << "_" << objIdx;
299		✗	std::string ig = obstacleName + ss.str();
300		✗	problemSolver()->addObstacle(ig, *(affs[objIdx]), false, false);
301		✗	hpp::pinocchio::CollisionObjectPtr_t obj = problemSolver()->obstacle(ig);
302		✗	objs.push_back(std::make_pair(ig, obj));
303		✗	}
304		✗	if (problemSolver()->affordanceObjects.has(ops[opIdx]->affordance_)) {
305			// std::vector<FclCollisionObjectSharePtr_t >
306			AffordanceObjects_t mapObjs =
307		✗	problemSolver()->affordanceObjects.get(ops[opIdx]->affordance_);
308		✗	objs.insert(objs.begin() + objs.size(), mapObjs.begin(), mapObjs.end());
309		✗	}
310		✗	problemSolver()->affordanceObjects.erase(ops[opIdx]->affordance_);
311		✗	problemSolver()->affordanceObjects.add(ops[opIdx]->affordance_, objs);
312		✗	}
313		✗	}
314
315		✗	hpp::doubleSeqSeqSeqSeq* Afford::getAffordancePoints(char const* affordance) {
316			hpp::doubleSeqSeqSeqSeq* affs;
317		✗	if (!problemSolver()->affordanceObjects.has(std::string(affordance))) {
318			throw hpp::Error(
319			"No affordance type of given name found. Unable to get affordance "
320		✗	"points.");
321			}
322			const AffordanceObjects_t& affObjs =
323		✗	problemSolver()->affordanceObjects.get(affordance);
324		✗	std::size_t nbAffs = affObjs.size();
325		✗	affs = new hpp::doubleSeqSeqSeqSeq();
326		✗	affs->length((CORBA::ULong)nbAffs);
327		✗	for (std::size_t affIdx = 0; affIdx < nbAffs; affIdx++) {
328			affordance::BVHModelOBConst_Ptr_t model =
329		✗	affordance::GetModel(affObjs[affIdx].second->fcl());
330		✗	std::size_t nbTris = model->num_tris;
331		✗	hpp::doubleSeqSeqSeq tris;
332		✗	tris.length((CORBA::ULong)nbTris);
333		✗	for (std::size_t triIdx = 0; triIdx < nbTris; triIdx++) {
334		✗	hpp::doubleSeqSeq triangle;
335		✗	const coal::Triangle& refTri = (*model->tri_indices)[triIdx];
336		✗	triangle.length(3);
337		✗	for (unsigned int vertIdx = 0; vertIdx < 3; vertIdx++) {
338		✗	coal::Vec3f p(affObjs[affIdx].second->fcl()->getRotation() *
339		✗	(*model->vertices)[refTri[vertIdx]] +
340		✗	affObjs[affIdx].second->fcl()->getTranslation());
341		✗	hpp::doubleSeq point;
342			// point always 3D
343		✗	point.length(3);
344		✗	for (std::size_t idx = 0; idx < 3; idx++) {
345		✗	point[(CORBA::ULong)idx] = p[idx];
346			}
347		✗	triangle[(CORBA::ULong)vertIdx] = point;
348		✗	}
349		✗	tris[(CORBA::ULong)triIdx] = triangle;
350		✗	}
351		✗	(*affs)[(CORBA::ULong)affIdx] = tris;
352		✗	}
353		✗	return affs;
354			}
355
356		✗	hpp::Names_t* fromStringVector(const std::vector<std::string>& input) {
357		✗	CORBA::ULong size = (CORBA::ULong)input.size();
358		✗	char** nameList = hpp::Names_t::allocbuf(size);
359		✗	hpp::Names_t* jointNames = new hpp::Names_t(size, size, nameList);
360		✗	for (std::size_t i = 0; i < input.size(); ++i) {
361		✗	std::string name = input[i];
362		✗	nameList[i] = (char*)malloc(sizeof(char) * (name.length() + 1));
363		✗	strcpy(nameList[i], name.c_str());
364		✗	}
365		✗	return jointNames;
366			}
367
368		✗	hpp::Names_t* Afford::getAffRefObstacles(const char* affordance) {
369		✗	std::vector<std::string> objList;
370		✗	if (!problemSolver()->affordanceObjects.has(std::string(affordance))) {
371			throw hpp::Error(
372			"No affordance type of given name found. Unable to get reference "
373		✗	"collision object.");
374			}
375			AffordanceObjects_t affObjs =
376		✗	problemSolver()->affordanceObjects.get(affordance);
377		✗	for (std::size_t affIdx = 0; affIdx < affObjs.size(); affIdx++) {
378			/*affordance::BVHModelOBConst_Ptr_t model =
379			affordance::GetModel (affObjs[affIdx]->fcl());*/
380		✗	objList.push_back(affObjs[affIdx].first);
381			}
382		✗	hpp::Names_t* objListPtr = fromStringVector(objList);
383		✗	return objListPtr;
384		✗	}
385
386		✗	hpp::Names_t* Afford::getAffordanceTypes() {
387			std::vector<std::string> affTypes =
388		✗	problemSolver()->affordanceObjects.getKeys<std::vector<std::string> >();
389		✗	if (affTypes.empty()) {
390		✗	throw hpp::Error("No affordances found. Return empty list.");
391			}
392		✗	hpp::Names_t* affTypeListPtr = fromStringVector(affTypes);
393		✗	return affTypeListPtr;
394		✗	}
395
396		✗	hpp::Names_t* Afford::getAffordanceConfigTypes() {
397			std::vector<std::string> affTypes =
398		✗	problemSolver()->affordanceConfigs.getKeys<std::vector<std::string> >();
399		✗	if (affTypes.empty()) {
400		✗	std::cout << "No affordance configs found. Return empty list." << std::endl;
401		✗	hpp::Names_t* empty = new hpp::Names_t();
402		✗	empty->length(0);
403		✗	return empty;
404			}
405		✗	hpp::Names_t* affTypeListPtr = fromStringVector(affTypes);
406		✗	return affTypeListPtr;
407		✗	}
408
409			} // namespace impl
410			} // namespace affordanceCorba
411			} // namespace hpp
412