19#ifndef HPP_RBPRM_SAMPLE_HH
20#define HPP_RBPRM_SAMPLE_HH
22#include <Eigen/StdVector>
24#include <hpp/pinocchio/device.hh>
42 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
51 Sample(
const pinocchio::JointPtr_t limb,
const pinocchio::Frame effector,
52 const fcl::Vec3f& offset = fcl::Vec3f(0, 0, 0),
53 const fcl::Vec3f& limbOffset = fcl::Vec3f(0, 0, 0),
54 const std::size_t
id = 0);
56 Sample(
const std::size_t
id,
const std::size_t length,
57 const std::size_t startRank,
const double staticValue,
58 const fcl::Vec3f& effectorPosition,
59 const fcl::Vec3f& effectorPositionInLimbFrame,
60 const pinocchio::ConfigurationIn_t configuration,
61 const Eigen::MatrixXd& jacobian,
62 const Eigen::Matrix<pinocchio::value_type, 6, 6>& jacobianProduct);
70 Sample(
const pinocchio::JointPtr_t limb,
const pinocchio::Frame effector,
71 pinocchio::ConfigurationIn_t configuration,
72 const fcl::Vec3f& offset = fcl::Vec3f(0, 0, 0),
73 const fcl::Vec3f& limbOffset = fcl::Vec3f(0, 0, 0),
74 const std::size_t
id = 0);
109 const pinocchio::JointPtr_t limb,
const std::string& effector,
110 const std::size_t nbSamples,
const fcl::Vec3f& offset = fcl::Vec3f(0, 0, 0),
111 const fcl::Vec3f& limbOffset = fcl::Vec3f(0, 0, 0));
116void Load(
const Sample& sample, pinocchio::ConfigurationOut_t robot);
double staticValue_
Definition: sample.hh:90
std::size_t length_
Definition: sample.hh:80
fcl::Vec3f effectorPosition_
Position relative to robot root (ie, robot base at 0 everywhere)
Definition: sample.hh:83
~Sample()
Definition: sample.hh:76
Sample(const pinocchio::JointPtr_t limb, const pinocchio::Frame effector, const fcl::Vec3f &offset=fcl::Vec3f(0, 0, 0), const fcl::Vec3f &limbOffset=fcl::Vec3f(0, 0, 0), const std::size_t id=0)
std::size_t id_
id in sample container
Definition: sample.hh:89
std::size_t startRank_
Definition: sample.hh:79
Sample(const pinocchio::JointPtr_t limb, const pinocchio::Frame effector, pinocchio::ConfigurationIn_t configuration, const fcl::Vec3f &offset=fcl::Vec3f(0, 0, 0), const fcl::Vec3f &limbOffset=fcl::Vec3f(0, 0, 0), const std::size_t id=0)
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 pinocchio::ConfigurationIn_t configuration, const Eigen::MatrixXd &jacobian, const Eigen::Matrix< pinocchio::value_type, 6, 6 > &jacobianProduct)
Eigen::MatrixXd jacobian_
Definition: sample.hh:85
fcl::Vec3f effectorPositionInLimbFrame_
Definition: sample.hh:84
Eigen::Matrix< pinocchio::value_type, 6, 6 > jacobianProduct_
Product of the jacobian by its transpose.
Definition: sample.hh:87
Sample(const Sample &clone)
pinocchio::Configuration_t configuration_
Definition: sample.hh:81
#define HPP_RBPRM_DLLAPI
Definition: config.hh:64
SampleVector_t GenerateSamples(const pinocchio::JointPtr_t limb, const std::string &effector, const std::size_t nbSamples, const fcl::Vec3f &offset=fcl::Vec3f(0, 0, 0), const fcl::Vec3f &limbOffset=fcl::Vec3f(0, 0, 0))
std::vector< Sample, Eigen::aligned_allocator< Sample > > SampleVector_t
Definition: sample.hh:100
shared_ptr< Sample > SamplePtr_t
Definition: sample.hh:37
void Load(const Sample &sample, pinocchio::ConfigurationOut_t robot)
HPP_PREDEF_CLASS(OctreeNode)
Definition: algorithm.hh:26
bool operator()(const Sample &lhs, const Sample &rhs) const
Definition: sample.hh:95