Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/sampling/sample.cc	Lines:	69	83	83.1 %
Date:	2024-02-02 12:21:48	Branches:	90	220	40.9 %


// 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/Eigen>
#include <hpp/core/configuration-shooter/uniform.hh>
#include <hpp/pinocchio/configuration.hh>
#include <hpp/pinocchio/joint.hh>
#include <hpp/rbprm/sampling/sample.hh>
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <pinocchio/multibody/geometry.hpp>

using namespace hpp;
using namespace hpp::pinocchio;
using namespace hpp::rbprm;
using namespace hpp::rbprm::sampling;

std::size_t ComputeLength(const hpp::pinocchio::JointPtr_t limb,
                          const hpp::pinocchio::Frame effector) {
  std::size_t start = limb->rankInConfiguration();
  std::size_t end =
      effector.joint()->rankInConfiguration() +
      effector.joint()->configSize();  // neutralConfiguration().rows();
  return end - start;
}

fcl::Vec3f ComputeEffectorPosition(const hpp::pinocchio::JointPtr_t limb,
                                   const hpp::pinocchio::Frame effector,
                                   const fcl::Vec3f& offset) {
  const hpp::pinocchio::Transform3f& tf = effector.currentTransformation();
  const fcl::Transform3f transform(tf.rotation(), tf.translation());
  hpp::pinocchio::Transform3f tfParent;
  if (limb->parentJoint())
    tfParent = limb->parentJoint()->currentTransformation();
  else
    tfParent = limb->currentTransformation();
  fcl::Transform3f parentT =
      fcl::Transform3f(tfParent.rotation(), tfParent.translation()).inverse();
  fcl::Vec3f tr(transform.getTranslation() + offset);
  return (parentT * tr).getTranslation();
}

/**
 * @brief ComputeEffectorPositionInLimbFrame compute the position of the end
 * effector in the frame defined by the world orientation but with the origin at
 * the root of the limb
 * @param limb
 * @param effector
 * @param offset
 * @return
 */
fcl::Vec3f ComputeEffectorPositionInLimbFrame(
    const hpp::pinocchio::JointPtr_t limb, const hpp::pinocchio::Frame effector,
    const fcl::Vec3f& offset, const fcl::Vec3f& limbOffset) {
  // we want to use the orientation expressed in the world frame, but with the
  // origin in the limb's root : hppDout(notice,"offset = "<<offset);
  // hppDout(notice,"limbOffset = "<<limbOffset);
  const hpp::pinocchio::Transform3f& transformLimbPin =
      limb->currentTransformation();
  const fcl::Transform3f transformLimb(transformLimbPin.rotation(),
                                       transformLimbPin.translation());
  fcl::Transform3f transformOffset;
  transformOffset.setTranslation(limbOffset);
  fcl::Vec3f effTr = effector.currentTransformation().translation() + offset;
  fcl::Vec3f limbTr = (transformLimb * transformOffset).getTranslation();
  /* hppDout(notice,"effTr = "<<effTr);
   hppDout(notice,"limbTr = "<<limbTr);
   hppDout(notice,"res = "<<effTr-limbTr);*/
  return (effTr - limbTr);
}

Eigen::MatrixXd Jacobian(const hpp::pinocchio::JointPtr_t limb,
                         const hpp::pinocchio::Frame effector) {
  return effector.jacobian().block(0, limb->rankInVelocity(), 6,
                                   effector.joint()->rankInVelocity() -
                                       limb->rankInVelocity() +
                                       effector.joint()->numberDof());
}

double Manipulability(const Eigen::MatrixXd& product) {
  double det = product.determinant();
  return det > 0 ? sqrt(det) : 0;
}

Sample::Sample(const hpp::pinocchio::JointPtr_t limb,
               const hpp::pinocchio::Frame effector, const fcl::Vec3f& offset,
               const fcl::Vec3f& limbOffset, std::size_t id)
    : startRank_(limb->rankInConfiguration()),
      length_(ComputeLength(limb, effector)),
      configuration_(
          limb->robot()->currentConfiguration().segment(startRank_, length_)),
      effectorPosition_(ComputeEffectorPosition(limb, effector, offset)),
      effectorPositionInLimbFrame_(ComputeEffectorPositionInLimbFrame(
          limb, effector, offset, limbOffset)),
      jacobian_(Jacobian(limb, effector)),
      jacobianProduct_(jacobian_ * jacobian_.transpose()),
      id_(id),
      staticValue_(Manipulability(jacobianProduct_)) {
  // NOTHING
}

Sample::Sample(
    const std::size_t id, const std::size_t length, const std::size_t startRank,
    const double staticValue, const fcl::Vec3f& effectorPosition,
    const fcl::Vec3f& effectorPositionInLimbFrame,
    const hpp::pinocchio::ConfigurationIn_t configuration,
    const Eigen::MatrixXd& jacobian,
    const Eigen::Matrix<hpp::pinocchio::value_type, 6, 6>& jacobianProduct)
    : startRank_(startRank),
      length_(length),
      configuration_(configuration),
      effectorPosition_(effectorPosition),
      effectorPositionInLimbFrame_(effectorPositionInLimbFrame),
      jacobian_(jacobian),
      jacobianProduct_(jacobianProduct),
      id_(id),
      staticValue_(staticValue) {
  // NOTHING
}

Sample::Sample(const hpp::pinocchio::JointPtr_t limb,
               const hpp::pinocchio::Frame effector,
               hpp::pinocchio::ConfigurationIn_t configuration,
               const fcl::Vec3f& offset, const fcl::Vec3f& limbOffset,
               std::size_t id)
    : startRank_(limb->rankInConfiguration()),
      length_(ComputeLength(limb, effector)),
      configuration_(configuration),
      effectorPosition_(ComputeEffectorPosition(limb, effector, offset)),
      effectorPositionInLimbFrame_(ComputeEffectorPositionInLimbFrame(
          limb, effector, offset, limbOffset)),
      jacobian_(Jacobian(limb, effector)),
      jacobianProduct_(jacobian_ * jacobian_.transpose()),
      id_(id),
      staticValue_(Manipulability(jacobianProduct_)) {
  // NOTHING
}

Sample::Sample(const Sample& clone)

    : startRank_(clone.startRank_),
      length_(clone.length_),
      configuration_(clone.configuration_),
      effectorPosition_(clone.effectorPosition_),
      effectorPositionInLimbFrame_(clone.effectorPositionInLimbFrame_),
      jacobian_(clone.jacobian_),
      jacobianProduct_(clone.jacobianProduct_),
      id_(clone.id_),
      staticValue_(clone.staticValue_) {
  // NOTHING
}

void hpp::rbprm::sampling::Load(const Sample& sample,
                                ConfigurationOut_t configuration) {
  configuration.segment(sample.startRank_, sample.length_) =
      sample.configuration_;
}

hpp::rbprm::sampling::SampleVector_t hpp::rbprm::sampling::GenerateSamples(
    const pinocchio::JointPtr_t model, const std::string& effector,
    const std::size_t nbSamples, const fcl::Vec3f& offset,
    const fcl::Vec3f& limbOffset) {
  SampleVector_t result;
  result.reserve(nbSamples);
  pinocchio::DevicePtr_t device(model->robot()->clone());
  Configuration_t config;
  JointPtr_t clone = device->getJointByName(model->name());
  Frame effectorClone = device->getFrameByName(effector);
  std::size_t startRank_(model->rankInConfiguration());
  std::size_t length_(ComputeLength(model, effectorClone));
  Configuration_t configRef(model->robot()->neutralConfiguration());
  hppDout(notice, "TEST REF CONFIG IN LIMB DB : "
                      << pinocchio::displayConfig(configRef));
  core::configurationShooter::UniformPtr_t shooter =
      core::configurationShooter::Uniform::create(device);
  result.push_back(Sample(clone, effectorClone,
                          configRef.segment(startRank_, length_), offset,
                          limbOffset, 0));
  for (std::size_t i = 1; i < nbSamples; ++i) {
    shooter->shoot(config);
    // clone->configuration ()->uniformlySample (clone->rankInConfiguration (),
    // config);
    /*Joint* current = clone;
    while(current->numberChildJoints() !=0)
    {
        current = current->childJoint(0);
        current->configuration ()->uniformlySample
    (current->rankInConfiguration(), config);
    }*/
    device->currentConfiguration(config);
    device->computeForwardKinematics();
    result.push_back(Sample(clone, effectorClone,
                            config.segment(startRank_, length_), offset,
                            limbOffset, i));
  }
  return result;
}


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/Eigen>
18			#include <hpp/core/configuration-shooter/uniform.hh>
19			#include <hpp/pinocchio/configuration.hh>
20			#include <hpp/pinocchio/joint.hh>
21			#include <hpp/rbprm/sampling/sample.hh>
22			#include <pinocchio/algorithm/joint-configuration.hpp>
23			#include <pinocchio/multibody/geometry.hpp>
24
25			using namespace hpp;
26			using namespace hpp::pinocchio;
27			using namespace hpp::rbprm;
28			using namespace hpp::rbprm::sampling;
29
30		10010	std::size_t ComputeLength(const hpp::pinocchio::JointPtr_t limb,
31			const hpp::pinocchio::Frame effector) {
32		10010	std::size_t start = limb->rankInConfiguration();
33			std::size_t end =
34	✓✗	10010	effector.joint()->rankInConfiguration() +
35	✓✗✓✗	10010	effector.joint()->configSize(); // neutralConfiguration().rows();
36		10010	return end - start;
37			}
38
39		10000	fcl::Vec3f ComputeEffectorPosition(const hpp::pinocchio::JointPtr_t limb,
40			const hpp::pinocchio::Frame effector,
41			const fcl::Vec3f& offset) {
42	✓✗	10000	const hpp::pinocchio::Transform3f& tf = effector.currentTransformation();
43	✓✗✓✗ ✓✗	10000	const fcl::Transform3f transform(tf.rotation(), tf.translation());
44	✓✗	10000	hpp::pinocchio::Transform3f tfParent;
45	✓✗✓✗	10000	if (limb->parentJoint())
46	✓✗✓✗ ✓✗	10000	tfParent = limb->parentJoint()->currentTransformation();
47			else
48			tfParent = limb->currentTransformation();
49			fcl::Transform3f parentT =
50	✓✗✓✗ ✓✗✓✗	10000	fcl::Transform3f(tfParent.rotation(), tfParent.translation()).inverse();
51	✓✗✓✗	10000	fcl::Vec3f tr(transform.getTranslation() + offset);
52	✓✗✓✗ ✓✗	20000	return (parentT * tr).getTranslation();
53			}
54
55			/**
56			* @brief ComputeEffectorPositionInLimbFrame compute the position of the end
57			* effector in the frame defined by the world orientation but with the origin at
58			* the root of the limb
59			* @param limb
60			* @param effector
61			* @param offset
62			* @return
63			*/
64		10000	fcl::Vec3f ComputeEffectorPositionInLimbFrame(
65			const hpp::pinocchio::JointPtr_t limb, const hpp::pinocchio::Frame effector,
66			const fcl::Vec3f& offset, const fcl::Vec3f& limbOffset) {
67			// we want to use the orientation expressed in the world frame, but with the
68			// origin in the limb's root : hppDout(notice,"offset = "<<offset);
69			// hppDout(notice,"limbOffset = "<<limbOffset);
70			const hpp::pinocchio::Transform3f& transformLimbPin =
71	✓✗	10000	limb->currentTransformation();
72		20000	const fcl::Transform3f transformLimb(transformLimbPin.rotation(),
73	✓✗✓✗ ✓✗	10000	transformLimbPin.translation());
74	✓✗	10000	fcl::Transform3f transformOffset;
75	✓✗	10000	transformOffset.setTranslation(limbOffset);
76	✓✗✓✗ ✓✗✓✗	10000	fcl::Vec3f effTr = effector.currentTransformation().translation() + offset;
77	✓✗✓✗	10000	fcl::Vec3f limbTr = (transformLimb * transformOffset).getTranslation();
78			/* hppDout(notice,"effTr = "<<effTr);
79			hppDout(notice,"limbTr = "<<limbTr);
80			hppDout(notice,"res = "<<effTr-limbTr);*/
81	✓✗✓✗	20000	return (effTr - limbTr);
82			}
83
84		10000	Eigen::MatrixXd Jacobian(const hpp::pinocchio::JointPtr_t limb,
85			const hpp::pinocchio::Frame effector) {
86		20000	return effector.jacobian().block(0, limb->rankInVelocity(), 6,
87	✓✗✓✗	20000	effector.joint()->rankInVelocity() -
88	✓✗	10000	limb->rankInVelocity() +
89	✓✗✓✗ ✓✗✓✗ ✓✗	30000	effector.joint()->numberDof());
90			}
91
92		10000	double Manipulability(const Eigen::MatrixXd& product) {
93		10000	double det = product.determinant();
94	✓✓	10000	return det > 0 ? sqrt(det) : 0;
95			}
96
97			Sample::Sample(const hpp::pinocchio::JointPtr_t limb,
98			const hpp::pinocchio::Frame effector, const fcl::Vec3f& offset,
99			const fcl::Vec3f& limbOffset, std::size_t id)
100			: startRank_(limb->rankInConfiguration()),
101			length_(ComputeLength(limb, effector)),
102			configuration_(
103			limb->robot()->currentConfiguration().segment(startRank_, length_)),
104			effectorPosition_(ComputeEffectorPosition(limb, effector, offset)),
105			effectorPositionInLimbFrame_(ComputeEffectorPositionInLimbFrame(
106			limb, effector, offset, limbOffset)),
107			jacobian_(Jacobian(limb, effector)),
108			jacobianProduct_(jacobian_ * jacobian_.transpose()),
109			id_(id),
110			staticValue_(Manipulability(jacobianProduct_)) {
111			// NOTHING
112			}
113
114			Sample::Sample(
115			const std::size_t id, const std::size_t length, const std::size_t startRank,
116			const double staticValue, const fcl::Vec3f& effectorPosition,
117			const fcl::Vec3f& effectorPositionInLimbFrame,
118			const hpp::pinocchio::ConfigurationIn_t configuration,
119			const Eigen::MatrixXd& jacobian,
120			const Eigen::Matrix<hpp::pinocchio::value_type, 6, 6>& jacobianProduct)
121			: startRank_(startRank),
122			length_(length),
123			configuration_(configuration),
124			effectorPosition_(effectorPosition),
125			effectorPositionInLimbFrame_(effectorPositionInLimbFrame),
126			jacobian_(jacobian),
127			jacobianProduct_(jacobianProduct),
128			id_(id),
129			staticValue_(staticValue) {
130			// NOTHING
131			}
132
133		10000	Sample::Sample(const hpp::pinocchio::JointPtr_t limb,
134			const hpp::pinocchio::Frame effector,
135			hpp::pinocchio::ConfigurationIn_t configuration,
136			const fcl::Vec3f& offset, const fcl::Vec3f& limbOffset,
137		10000	std::size_t id)
138		10000	: startRank_(limb->rankInConfiguration()),
139	✓✗	20000	length_(ComputeLength(limb, effector)),
140			configuration_(configuration),
141			effectorPosition_(ComputeEffectorPosition(limb, effector, offset)),
142			effectorPositionInLimbFrame_(ComputeEffectorPositionInLimbFrame(
143			limb, effector, offset, limbOffset)),
144			jacobian_(Jacobian(limb, effector)),
145	✓✗	10000	jacobianProduct_(jacobian_ * jacobian_.transpose()),
146			id_(id),
147	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	40000	staticValue_(Manipulability(jacobianProduct_)) {
148			// NOTHING
149		10000	}
150
151		49089	Sample::Sample(const Sample& clone)
152
153		49089	: startRank_(clone.startRank_),
154		49089	length_(clone.length_),
155		49089	configuration_(clone.configuration_),
156		49089	effectorPosition_(clone.effectorPosition_),
157		49089	effectorPositionInLimbFrame_(clone.effectorPositionInLimbFrame_),
158		49089	jacobian_(clone.jacobian_),
159		49089	jacobianProduct_(clone.jacobianProduct_),
160		49089	id_(clone.id_),
161	✓✗✓✗ ✓✗✓✗	49089	staticValue_(clone.staticValue_) {
162			// NOTHING
163		49089	}
164
165			void hpp::rbprm::sampling::Load(const Sample& sample,
166			ConfigurationOut_t configuration) {
167			configuration.segment(sample.startRank_, sample.length_) =
168			sample.configuration_;
169			}
170
171		10	hpp::rbprm::sampling::SampleVector_t hpp::rbprm::sampling::GenerateSamples(
172			const pinocchio::JointPtr_t model, const std::string& effector,
173			const std::size_t nbSamples, const fcl::Vec3f& offset,
174			const fcl::Vec3f& limbOffset) {
175	✓✗	10	SampleVector_t result;
176	✓✗	10	result.reserve(nbSamples);
177	✓✗	20	pinocchio::DevicePtr_t device(model->robot()->clone());
178	✓✗	20	Configuration_t config;
179	✓✗✓✗	20	JointPtr_t clone = device->getJointByName(model->name());
180	✓✗	20	Frame effectorClone = device->getFrameByName(effector);
181	✓✗	10	std::size_t startRank_(model->rankInConfiguration());
182	✓✗✓✗	10	std::size_t length_(ComputeLength(model, effectorClone));
183	✓✗	20	Configuration_t configRef(model->robot()->neutralConfiguration());
184			hppDout(notice, "TEST REF CONFIG IN LIMB DB : "
185			<< pinocchio::displayConfig(configRef));
186			core::configurationShooter::UniformPtr_t shooter =
187	✓✗	20	core::configurationShooter::Uniform::create(device);
188	✓✗✓✗ ✓✗✓✗	10	result.push_back(Sample(clone, effectorClone,
189	✓✗	10	configRef.segment(startRank_, length_), offset,
190			limbOffset, 0));
191	✓✓	10000	for (std::size_t i = 1; i < nbSamples; ++i) {
192	✓✗	9990	shooter->shoot(config);
193			// clone->configuration ()->uniformlySample (clone->rankInConfiguration (),
194			// config);
195			/Joint current = clone;
196			while(current->numberChildJoints() !=0)
197			{
198			current = current->childJoint(0);
199			current->configuration ()->uniformlySample
200			(current->rankInConfiguration(), config);
201			}*/
202	✓✗✓✗	9990	device->currentConfiguration(config);
203	✓✗	9990	device->computeForwardKinematics();
204	✓✗✓✗ ✓✗✓✗	9990	result.push_back(Sample(clone, effectorClone,
205	✓✗	19980	config.segment(startRank_, length_), offset,
206			limbOffset, i));
207			}
208		20	return result;
209			}