Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/contact_generation/kinematics_constraints.cc	Lines:	68	121	56.2 %
Date:	2024-02-02 12:21:48	Branches:	86	276	31.2 %


#include <hpp/fcl/BVH/BVH_model.h>
#include <hpp/fcl/mesh_loader/assimp.h>

#include <hpp/pinocchio/urdf/util.hh>
#include <hpp/rbprm/contact_generation/kinematics_constraints.hh>
#include <hpp/rbprm/rbprm-device.hh>
#include <hpp/rbprm/rbprm-fullbody.hh>
#include <pinocchio/multibody/geometry.hpp>
#include <pinocchio/parsers/utils.hpp>
#include <pinocchio/utils/file-explorer.hpp>

namespace hpp {
namespace rbprm {
namespace reachability {

typedef fcl::BVHModel<fcl::OBBRSS> PolyhedronType;
typedef fcl::shared_ptr<PolyhedronType> PolyhedronPtrType;

VectorX triangleNormal(const PolyhedronPtrType& obj, size_t index) {
  VectorX normal(3);
  // access a vertice of faceId with : obj->vertices[obj->tri_indices[index](i)]
  // outward normal of triangle : (tri.p2 - tri.p1).cross(tri.p3 - tri.p1);
  /*hppDout(notice,"Face : "<<index);
  hppDout(notice,"0 - "<<obj->vertices[obj->tri_indices[index][0]]);
  hppDout(notice,"1 - "<<obj->vertices[obj->tri_indices[index][1]]);
  hppDout(notice,"2 - "<<obj->vertices[obj->tri_indices[index][2]]);
  */
  normal = (obj->vertices[obj->tri_indices[index][1]] -
            obj->vertices[obj->tri_indices[index][0]])
               .cross(obj->vertices[obj->tri_indices[index][2]] -
                      obj->vertices[obj->tri_indices[index][0]]);
  return normal.normalized();
}

VectorX triangleNormalTransform(const PolyhedronPtrType& obj, size_t index,
                                fcl::Transform3f T) {
  VectorX normal(3);
  // access a vertice of faceId with : obj->vertices[obj->tri_indices[index](i)]
  // outward normal of triangle : (tri.p2 - tri.p1).cross(tri.p3 - tri.p1);
  /*hppDout(notice,"Face : "<<index);
  hppDout(notice,"0 - "<<obj->vertices[obj->tri_indices[index][0]]);
  hppDout(notice,"1 - "<<obj->vertices[obj->tri_indices[index][1]]);
  hppDout(notice,"2 - "<<obj->vertices[obj->tri_indices[index][2]]);
  */
  fcl::Vec3f t0, t1, t2;
  t0 = T.transform(obj->vertices[obj->tri_indices[index][0]]);
  t1 = T.transform(obj->vertices[obj->tri_indices[index][1]]);
  t2 = T.transform(obj->vertices[obj->tri_indices[index][2]]);
  normal = (t1 - t0).cross(t2 - t0);
  return normal.normalized();
}

}  // namespace reachability
}  // namespace rbprm
}  // namespace hpp

std::pair<hpp::rbprm::MatrixX3, hpp::rbprm::MatrixX3>
hpp::rbprm::reachability::loadConstraintsFromObj(const std::string& fileName,
                                                 double minDistance) {
  hppDout(notice, "Load constraints for filename : " << fileName);

  std::vector<std::string> package_dirs = ::pinocchio::rosPaths();
  std::string meshPath =
      ::pinocchio::retrieveResourcePath(fileName, package_dirs);
  if (meshPath == "") {
    hppDout(warning, "Unable to load kinematics constraints : "
                         << "Mesh " << fileName << " could not be found.");
    return std::pair<hpp::rbprm::MatrixX3, hpp::rbprm::MatrixX3>();
  }

  // pinocchio::urdf::Parser parser("anchor",pinocchio::DevicePtr_t());
  hpp::rbprm::reachability::PolyhedronPtrType polyhedron(
      new hpp::rbprm::reachability::PolyhedronType);
  // name is stored in link->name
  try {
    fcl::loadPolyhedronFromResource(meshPath, hpp::rbprm::Vector3(1, 1, 1),
                                    polyhedron);
  } catch (std::runtime_error e) {
    hppDout(warning, "Unable to load kinematics constraints : " << e.what());
    return std::pair<hpp::rbprm::MatrixX3, hpp::rbprm::MatrixX3>();
  }

  // iterate over all faces : for each faces add a line in A : normal and a
  // value in b : position of a vertice.dot(normal)
  size_t numFaces = polyhedron->num_tris;
  hppDout(notice, "Num faces : " << numFaces);
  size_t numIneq = numFaces;
  if (minDistance > 0) {
    numIneq++;
  }
  hpp::rbprm::MatrixX3 N(numIneq, 3);
  hpp::rbprm::MatrixX3 V(numIneq, 3);

  hpp::rbprm::VectorX n, v;
  for (size_t fId = 0; fId < numFaces; ++fId) {
    // hppDout(notice,"For face : "<<fId);
    n = hpp::rbprm::reachability::triangleNormal(polyhedron, fId);
    v = polyhedron->vertices[polyhedron->tri_indices[fId][0]];
    // hppDout(notice,"normal : "<<n.transpose());
    // hppDout(notice,"vertice: "<<v.transpose());
    N.block<1, 3>(fId, 0) = n;
    V.block<1, 3>(fId, 0) = v;
  }

  if (minDistance > 0) {
    N.block<1, 3>(numIneq - 1, 0) = hpp::rbprm::Vector3(0, 0, -1);
    V.block<1, 3>(numIneq - 1, 0) = hpp::rbprm::Vector3(0, 0, minDistance);
  }
  hppDout(notice, "End of loading kinematic constraints : ");
  // hppDout(notice,"N : "<<N);
  // hppDout(notice,"v : "<<V);

  return std::make_pair(N, V);
}

namespace hpp {
namespace rbprm {
namespace reachability {
std::pair<MatrixX3, VectorX> computeAllKinematicsConstraints(
    const RbPrmFullBodyPtr_t& fullBody,
    const pinocchio::ConfigurationPtr_t& configuration) {
  fullBody->device_->currentConfiguration(*configuration);
  fullBody->device_->computeForwardKinematics();
  // first loop to compute size required :
  size_t numIneq = 0;
  for (CIT_Limb lit = fullBody->GetLimbs().begin();
       lit != fullBody->GetLimbs().end(); ++lit) {
    numIneq += lit->second->kinematicConstraints_.first.rows();
  }
  MatrixX3 A(numIneq, 3);
  VectorX b(numIneq);
  std::pair<MatrixX3, VectorX> Ab_limb;
  size_t currentId = 0;
  for (CIT_Limb lit = fullBody->GetLimbs().begin();
       lit != fullBody->GetLimbs().end(); ++lit) {
    if (lit->second->kinematicConstraints_.first.size() > 0) {
      Ab_limb = getInequalitiesAtTransform(
          lit->second->kinematicConstraints_,
          lit->second->effector_.currentTransformation());
      A.block(currentId, 0, Ab_limb.first.rows(), 3) = Ab_limb.first;
      b.segment(currentId, Ab_limb.first.rows()) = Ab_limb.second;
      currentId += Ab_limb.first.rows();
    }
  }

  return std::make_pair(A, b);
}

std::pair<MatrixX3, VectorX> computeKinematicsConstraintsForState(
    const RbPrmFullBodyPtr_t& fullBody, const State& state) {
  fullBody->device_->currentConfiguration(state.configuration_);
  fullBody->device_->computeForwardKinematics();
  hppDout(notice, "Compute kinematics constraints :");
  // first loop to compute size required :
  size_t numIneq = 0;
  for (std::map<std::string, bool>::const_iterator cit =
           state.contacts_.begin();
       cit != state.contacts_.end(); ++cit) {
    if (cit->second) {  // limb with name cit->first is in contact
      numIneq +=
          fullBody->GetLimb(cit->first)->kinematicConstraints_.first.rows();
      hppDout(notice, "Limb : " << cit->first << " is in contact.");
    }
  }
  MatrixX3 A(numIneq, 3);
  VectorX b(numIneq);
  std::pair<MatrixX3, VectorX> Ab_limb;
  size_t currentId = 0;
  RbPrmLimbPtr_t limb;
  for (std::map<std::string, bool>::const_iterator cit =
           state.contacts_.begin();
       cit != state.contacts_.end(); ++cit) {
    if (cit->second) {  // limb with name cit->first is in contact
      limb = fullBody->GetLimb(cit->first);
      Ab_limb = getInequalitiesAtTransform(
          limb->kinematicConstraints_, limb->effector_.currentTransformation());
      A.block(currentId, 0, Ab_limb.first.rows(), 3) = Ab_limb.first;
      b.segment(currentId, Ab_limb.first.rows()) = Ab_limb.second;
      currentId += Ab_limb.first.rows();
    }
  }
  hppDout(notice, "End of kinematics constraints, A size : ("
                      << A.rows() << "," << A.cols() << ")");
  return std::make_pair(A, b);
}

std::pair<MatrixX3, VectorX> computeKinematicsConstraintsForLimb(
    const RbPrmFullBodyPtr_t& fullBody, const State& state,
    const std::string& limbName) {
  fullBody->device_->currentConfiguration(state.configuration_);
  fullBody->device_->computeForwardKinematics();
  hppDout(notice, "Compute kinematics constraints for limb :" << limbName);
  // first loop to compute size required :
  if (state.contacts_.find(limbName) == state.contacts_.end()) {
    hppDout(warning, "No Limbs in contact found with name :" << limbName);
    return std::pair<MatrixX3, VectorX>();
  }
  if (!state.contacts_.at(limbName)) {
    hppDout(warning,
            "Limb " << limbName << " is not in contact for current state");
    return std::pair<MatrixX3, VectorX>();
  }

  RbPrmLimbPtr_t limb = fullBody->GetLimb(limbName);
  return getInequalitiesAtTransform(limb->kinematicConstraints_,
                                    limb->effector_.currentTransformation());
}

std::pair<MatrixX3, VectorX> getInequalitiesAtTransform(
    const std::pair<MatrixX3, MatrixX3>& NV,
    const hpp::pinocchio::Transform3f& transformPin) {
  fcl::Transform3f transform(transformPin.rotation(),
                             transformPin.translation());
  size_t numIneq = NV.first.rows();
  MatrixX3 A(numIneq, 3);
  VectorX b(numIneq);
  VectorX n, v;
  for (size_t i = 0; i < numIneq; ++i) {
    n = transform.getRotation() * (NV.first.block<1, 3>(i, 0).transpose());
    v = transform.transform(NV.second.block<1, 3>(i, 0).transpose());
    A.block<1, 3>(i, 0) = n;
    b[i] = v.dot(n);
  }
  // hppDout(notice,"Transform : "<<transform);
  //  hppDout(notice,"A : "<<A);
  //  hppDout(notice,"b : "<<b);
  return std::make_pair(A, b);
}

bool verifyKinematicConstraints(const std::pair<MatrixX3, MatrixX3>& NV,
                                const hpp::pinocchio::Transform3f& transform,
                                const fcl::Vec3f& point) {
  return verifyKinematicConstraints(getInequalitiesAtTransform(NV, transform),
                                    point);
}

bool verifyKinematicConstraints(const std::pair<MatrixX3, VectorX>& Ab,
                                const fcl::Vec3f& point) {
  hppDout(notice, "verify kinematic constraints : point = " << point);
  for (size_type i = 0; i < Ab.second.size(); ++i) {
    hppDout(notice, "for i = " << i);
    hppDout(notice, "A = " << (Ab.first.block<1, 3>(i, 0)));
    hppDout(notice, "b = " << Ab.second[i]);
    if (Ab.first.block<1, 3>(i, 0).dot(point) > Ab.second[i]) {
      hppDout(notice, "Constraint not verified !");
      return false;
    }
  }
  return true;
}

bool verifyKinematicConstraints(const RbPrmFullBodyPtr_t& fullbody,
                                const State& state, fcl::Vec3f point) {
  return verifyKinematicConstraints(
      computeKinematicsConstraintsForState(fullbody, state), point);
}

}  // namespace reachability
}  // namespace rbprm
}  // namespace hpp


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			#include <hpp/fcl/BVH/BVH_model.h>
2			#include <hpp/fcl/mesh_loader/assimp.h>
3
4			#include <hpp/pinocchio/urdf/util.hh>
5			#include <hpp/rbprm/contact_generation/kinematics_constraints.hh>
6			#include <hpp/rbprm/rbprm-device.hh>
7			#include <hpp/rbprm/rbprm-fullbody.hh>
8			#include <pinocchio/multibody/geometry.hpp>
9			#include <pinocchio/parsers/utils.hpp>
10			#include <pinocchio/utils/file-explorer.hpp>
11
12			namespace hpp {
13			namespace rbprm {
14			namespace reachability {
15
16			typedef fcl::BVHModel<fcl::OBBRSS> PolyhedronType;
17			typedef fcl::shared_ptr<PolyhedronType> PolyhedronPtrType;
18
19		1166	VectorX triangleNormal(const PolyhedronPtrType& obj, size_t index) {
20	✓✗	2332	VectorX normal(3);
21			// access a vertice of faceId with : obj->vertices[obj->tri_indices[index](i)]
22			// outward normal of triangle : (tri.p2 - tri.p1).cross(tri.p3 - tri.p1);
23			/*hppDout(notice,"Face : "<<index);
24			hppDout(notice,"0 - "<<obj->vertices[obj->tri_indices[index][0]]);
25			hppDout(notice,"1 - "<<obj->vertices[obj->tri_indices[index][1]]);
26			hppDout(notice,"2 - "<<obj->vertices[obj->tri_indices[index][2]]);
27			*/
28	✓✗	1166	normal = (obj->vertices[obj->tri_indices[index][1]] -
29		1166	obj->vertices[obj->tri_indices[index][0]])
30	✓✗✓✗	1166	.cross(obj->vertices[obj->tri_indices[index][2]] -
31	✓✗	2332	obj->vertices[obj->tri_indices[index][0]]);
32	✓✗	2332	return normal.normalized();
33			}
34
35			VectorX triangleNormalTransform(const PolyhedronPtrType& obj, size_t index,
36			fcl::Transform3f T) {
37			VectorX normal(3);
38			// access a vertice of faceId with : obj->vertices[obj->tri_indices[index](i)]
39			// outward normal of triangle : (tri.p2 - tri.p1).cross(tri.p3 - tri.p1);
40			/*hppDout(notice,"Face : "<<index);
41			hppDout(notice,"0 - "<<obj->vertices[obj->tri_indices[index][0]]);
42			hppDout(notice,"1 - "<<obj->vertices[obj->tri_indices[index][1]]);
43			hppDout(notice,"2 - "<<obj->vertices[obj->tri_indices[index][2]]);
44			*/
45			fcl::Vec3f t0, t1, t2;
46			t0 = T.transform(obj->vertices[obj->tri_indices[index][0]]);
47			t1 = T.transform(obj->vertices[obj->tri_indices[index][1]]);
48			t2 = T.transform(obj->vertices[obj->tri_indices[index][2]]);
49			normal = (t1 - t0).cross(t2 - t0);
50			return normal.normalized();
51			}
52
53			} // namespace reachability
54			} // namespace rbprm
55			} // namespace hpp
56
57			std::pair<hpp::rbprm::MatrixX3, hpp::rbprm::MatrixX3>
58		10	hpp::rbprm::reachability::loadConstraintsFromObj(const std::string& fileName,
59			double minDistance) {
60			hppDout(notice, "Load constraints for filename : " << fileName);
61
62	✓✗	20	std::vector<std::string> package_dirs = ::pinocchio::rosPaths();
63			std::string meshPath =
64	✓✗	20	::pinocchio::retrieveResourcePath(fileName, package_dirs);
65	✗✓	10	if (meshPath == "") {
66			hppDout(warning, "Unable to load kinematics constraints : "
67			<< "Mesh " << fileName << " could not be found.");
68			return std::pair<hpp::rbprm::MatrixX3, hpp::rbprm::MatrixX3>();
69			}
70
71			// pinocchio::urdf::Parser parser("anchor",pinocchio::DevicePtr_t());
72			hpp::rbprm::reachability::PolyhedronPtrType polyhedron(
73	✓✗✓✗ ✓✗	20	new hpp::rbprm::reachability::PolyhedronType);
74			// name is stored in link->name
75			try {
76	✓✗✓✗	10	fcl::loadPolyhedronFromResource(meshPath, hpp::rbprm::Vector3(1, 1, 1),
77			polyhedron);
78			} catch (std::runtime_error e) {
79			hppDout(warning, "Unable to load kinematics constraints : " << e.what());
80			return std::pair<hpp::rbprm::MatrixX3, hpp::rbprm::MatrixX3>();
81			}
82
83			// iterate over all faces : for each faces add a line in A : normal and a
84			// value in b : position of a vertice.dot(normal)
85		10	size_t numFaces = polyhedron->num_tris;
86			hppDout(notice, "Num faces : " << numFaces);
87		10	size_t numIneq = numFaces;
88	✓✗	10	if (minDistance > 0) {
89		10	numIneq++;
90			}
91	✓✗	20	hpp::rbprm::MatrixX3 N(numIneq, 3);
92	✓✗	20	hpp::rbprm::MatrixX3 V(numIneq, 3);
93
94	✓✗✓✗	20	hpp::rbprm::VectorX n, v;
95	✓✓	1176	for (size_t fId = 0; fId < numFaces; ++fId) {
96			// hppDout(notice,"For face : "<<fId);
97	✓✗	1166	n = hpp::rbprm::reachability::triangleNormal(polyhedron, fId);
98	✓✗	1166	v = polyhedron->vertices[polyhedron->tri_indices[fId][0]];
99			// hppDout(notice,"normal : "<<n.transpose());
100			// hppDout(notice,"vertice: "<<v.transpose());
101	✓✗✓✗	1166	N.block<1, 3>(fId, 0) = n;
102	✓✗✓✗	1166	V.block<1, 3>(fId, 0) = v;
103			}
104
105	✓✗	10	if (minDistance > 0) {
106	✓✗✓✗ ✓✗	10	N.block<1, 3>(numIneq - 1, 0) = hpp::rbprm::Vector3(0, 0, -1);
107	✓✗✓✗ ✓✗	10	V.block<1, 3>(numIneq - 1, 0) = hpp::rbprm::Vector3(0, 0, minDistance);
108			}
109			hppDout(notice, "End of loading kinematic constraints : ");
110			// hppDout(notice,"N : "<<N);
111			// hppDout(notice,"v : "<<V);
112
113	✓✗	10	return std::make_pair(N, V);
114			}
115
116			namespace hpp {
117			namespace rbprm {
118			namespace reachability {
119			std::pair<MatrixX3, VectorX> computeAllKinematicsConstraints(
120			const RbPrmFullBodyPtr_t& fullBody,
121			const pinocchio::ConfigurationPtr_t& configuration) {
122			fullBody->device_->currentConfiguration(*configuration);
123			fullBody->device_->computeForwardKinematics();
124			// first loop to compute size required :
125			size_t numIneq = 0;
126			for (CIT_Limb lit = fullBody->GetLimbs().begin();
127			lit != fullBody->GetLimbs().end(); ++lit) {
128			numIneq += lit->second->kinematicConstraints_.first.rows();
129			}
130			MatrixX3 A(numIneq, 3);
131			VectorX b(numIneq);
132			std::pair<MatrixX3, VectorX> Ab_limb;
133			size_t currentId = 0;
134			for (CIT_Limb lit = fullBody->GetLimbs().begin();
135			lit != fullBody->GetLimbs().end(); ++lit) {
136			if (lit->second->kinematicConstraints_.first.size() > 0) {
137			Ab_limb = getInequalitiesAtTransform(
138			lit->second->kinematicConstraints_,
139			lit->second->effector_.currentTransformation());
140			A.block(currentId, 0, Ab_limb.first.rows(), 3) = Ab_limb.first;
141			b.segment(currentId, Ab_limb.first.rows()) = Ab_limb.second;
142			currentId += Ab_limb.first.rows();
143			}
144			}
145
146			return std::make_pair(A, b);
147			}
148
149		32	std::pair<MatrixX3, VectorX> computeKinematicsConstraintsForState(
150			const RbPrmFullBodyPtr_t& fullBody, const State& state) {
151	✓✗✓✗	32	fullBody->device_->currentConfiguration(state.configuration_);
152	✓✗	32	fullBody->device_->computeForwardKinematics();
153			hppDout(notice, "Compute kinematics constraints :");
154			// first loop to compute size required :
155		32	size_t numIneq = 0;
156		64	for (std::map<std::string, bool>::const_iterator cit =
157		32	state.contacts_.begin();
158	✓✓	160	cit != state.contacts_.end(); ++cit) {
159	✓✗	64	if (cit->second) { // limb with name cit->first is in contact
160		64	numIneq +=
161	✓✗✓✗ ✓✗	64	fullBody->GetLimb(cit->first)->kinematicConstraints_.first.rows();
162			hppDout(notice, "Limb : " << cit->first << " is in contact.");
163			}
164			}
165	✓✗	64	MatrixX3 A(numIneq, 3);
166	✓✗	64	VectorX b(numIneq);
167	✓✗	64	std::pair<MatrixX3, VectorX> Ab_limb;
168		32	size_t currentId = 0;
169		32	RbPrmLimbPtr_t limb;
170		64	for (std::map<std::string, bool>::const_iterator cit =
171		32	state.contacts_.begin();
172	✓✓	160	cit != state.contacts_.end(); ++cit) {
173	✓✗	64	if (cit->second) { // limb with name cit->first is in contact
174	✓✗✓✗	64	limb = fullBody->GetLimb(cit->first);
175	✓✗	128	Ab_limb = getInequalitiesAtTransform(
176	✓✗✓✗	128	limb->kinematicConstraints_, limb->effector_.currentTransformation());
177	✓✗✓✗ ✓✗	64	A.block(currentId, 0, Ab_limb.first.rows(), 3) = Ab_limb.first;
178	✓✗✓✗ ✓✗	64	b.segment(currentId, Ab_limb.first.rows()) = Ab_limb.second;
179	✓✗	64	currentId += Ab_limb.first.rows();
180			}
181			}
182			hppDout(notice, "End of kinematics constraints, A size : ("
183			<< A.rows() << "," << A.cols() << ")");
184	✓✗	64	return std::make_pair(A, b);
185			}
186
187			std::pair<MatrixX3, VectorX> computeKinematicsConstraintsForLimb(
188			const RbPrmFullBodyPtr_t& fullBody, const State& state,
189			const std::string& limbName) {
190			fullBody->device_->currentConfiguration(state.configuration_);
191			fullBody->device_->computeForwardKinematics();
192			hppDout(notice, "Compute kinematics constraints for limb :" << limbName);
193			// first loop to compute size required :
194			if (state.contacts_.find(limbName) == state.contacts_.end()) {
195			hppDout(warning, "No Limbs in contact found with name :" << limbName);
196			return std::pair<MatrixX3, VectorX>();
197			}
198			if (!state.contacts_.at(limbName)) {
199			hppDout(warning,
200			"Limb " << limbName << " is not in contact for current state");
201			return std::pair<MatrixX3, VectorX>();
202			}
203
204			RbPrmLimbPtr_t limb = fullBody->GetLimb(limbName);
205			return getInequalitiesAtTransform(limb->kinematicConstraints_,
206			limb->effector_.currentTransformation());
207			}
208
209		64	std::pair<MatrixX3, VectorX> getInequalitiesAtTransform(
210			const std::pair<MatrixX3, MatrixX3>& NV,
211			const hpp::pinocchio::Transform3f& transformPin) {
212		128	fcl::Transform3f transform(transformPin.rotation(),
213	✓✗✓✗ ✓✗	64	transformPin.translation());
214	✓✗	64	size_t numIneq = NV.first.rows();
215	✓✗	128	MatrixX3 A(numIneq, 3);
216	✓✗	128	VectorX b(numIneq);
217	✓✗✓✗	128	VectorX n, v;
218	✓✓	9280	for (size_t i = 0; i < numIneq; ++i) {
219	✓✗✓✗ ✓✗✓✗	9216	n = transform.getRotation() * (NV.first.block<1, 3>(i, 0).transpose());
220	✓✗✓✗ ✓✗✓✗	9216	v = transform.transform(NV.second.block<1, 3>(i, 0).transpose());
221	✓✗✓✗	9216	A.block<1, 3>(i, 0) = n;
222	✓✗✓✗	9216	b[i] = v.dot(n);
223			}
224			// hppDout(notice,"Transform : "<<transform);
225			// hppDout(notice,"A : "<<A);
226			// hppDout(notice,"b : "<<b);
227	✓✗	128	return std::make_pair(A, b);
228			}
229
230			bool verifyKinematicConstraints(const std::pair<MatrixX3, MatrixX3>& NV,
231			const hpp::pinocchio::Transform3f& transform,
232			const fcl::Vec3f& point) {
233			return verifyKinematicConstraints(getInequalitiesAtTransform(NV, transform),
234			point);
235			}
236
237			bool verifyKinematicConstraints(const std::pair<MatrixX3, VectorX>& Ab,
238			const fcl::Vec3f& point) {
239			hppDout(notice, "verify kinematic constraints : point = " << point);
240			for (size_type i = 0; i < Ab.second.size(); ++i) {
241			hppDout(notice, "for i = " << i);
242			hppDout(notice, "A = " << (Ab.first.block<1, 3>(i, 0)));
243			hppDout(notice, "b = " << Ab.second[i]);
244			if (Ab.first.block<1, 3>(i, 0).dot(point) > Ab.second[i]) {
245			hppDout(notice, "Constraint not verified !");
246			return false;
247			}
248			}
249			return true;
250			}
251
252			bool verifyKinematicConstraints(const RbPrmFullBodyPtr_t& fullbody,
253			const State& state, fcl::Vec3f point) {
254			return verifyKinematicConstraints(
255			computeKinematicsConstraintsForState(fullbody, state), point);
256			}
257
258			} // namespace reachability
259			} // namespace rbprm
260			} // namespace hpp