Commit 7207542c authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Replace double by value_type.

parent 769cabc1
......@@ -178,13 +178,13 @@ namespace hpp {
}
/// Get mass.
inline double mass() const
inline value_type mass() const
{
return mass_;
}
/// Set mass.
inline void mass(double mass)
inline void mass(value_type mass)
{
mass_ = mass;
}
......@@ -201,7 +201,7 @@ namespace hpp {
/// Inertial information
fcl::Vec3f localCom_;
matrix3_t inertiaMatrix_;
double mass_;
value_type mass_;
value_type radius_;
}; // class Body
} // namespace model
......
......@@ -203,7 +203,7 @@ namespace hpp {
/// \{
/// Get mass of robot
const double& mass () const
const value_type& mass () const
{
return mass_;
}
......@@ -342,7 +342,7 @@ namespace hpp {
vector_t currentAcceleration_;
vector3_t com_;
ComJacobian_t jacobianCom_;
double mass_;
value_type mass_;
bool upToDate_;
Computation_t computationFlag_;
// Collision pairs between bodies
......
......@@ -29,7 +29,7 @@ namespace hpp {
/// Result of distance computation between two CollisionObject.
struct HPP_MODEL_DLLAPI DistanceResult {
/// Get distance between objects
const double& distance () const
const value_type& distance () const
{
return fcl.min_distance;
}
......
......@@ -69,8 +69,8 @@ namespace hpp {
dimension_ = dimension;
lowerBounds_.resize (dimension);
upperBounds_.resize (dimension);
lowerBounds_.setConstant (-std::numeric_limits<double>::infinity());
upperBounds_.setConstant (+std::numeric_limits<double>::infinity());
lowerBounds_.setConstant (-std::numeric_limits<value_type>::infinity());
upperBounds_.setConstant (+std::numeric_limits<value_type>::infinity());
}
size_type dimension_;
vector_t lowerBounds_;
......
......@@ -47,19 +47,19 @@ namespace hpp {
enum Request_t {COLLISION, DISTANCE};
typedef double value_type;
typedef Eigen::Matrix <double, Eigen::Dynamic, 1> vector_t;
typedef Eigen::Matrix <value_type, Eigen::Dynamic, 1> vector_t;
typedef vector_t Configuration_t;
typedef Eigen::Ref <const Configuration_t> ConfigurationIn_t;
typedef Eigen::Ref <Configuration_t> ConfigurationOut_t;
typedef Eigen::Ref <const vector_t> vectorIn_t;
typedef Eigen::Ref <vector_t> vectorOut_t;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic> matrix_t;
typedef Eigen::Ref <matrix_t> matrixOut_t;
typedef matrix_t::Index size_type;
typedef fcl::Matrix3f matrix3_t;
typedef fcl::Vec3f vector3_t;
typedef Eigen::Matrix <double, 6, Eigen::Dynamic> JointJacobian_t;
typedef Eigen::Matrix <double, 3, Eigen::Dynamic> ComJacobian_t;
typedef Eigen::Matrix <value_type, 6, Eigen::Dynamic> JointJacobian_t;
typedef Eigen::Matrix <value_type, 3, Eigen::Dynamic> ComJacobian_t;
typedef Eigen::Block <JointJacobian_t, 3, Eigen::Dynamic>
HalfJointJacobian_t;
......
......@@ -63,7 +63,7 @@ namespace hpp {
/// \li x, y, z, roll, pitch, yaw for freeflyer joints.
virtual void interpolate (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const double& u,
const value_type& u,
const size_type& index,
ConfigurationOut_t result) = 0;
......@@ -71,9 +71,9 @@ namespace hpp {
/// \param q1, q2 two configurations of the robot
/// \param index index of first component of q1 and q2 corresponding to
/// the joint.
virtual double distance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const = 0;
virtual value_type distance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const = 0;
/// Integrate constant derivative during unit time
/// \param q initial configuration
......@@ -123,13 +123,13 @@ namespace hpp {
/// Get whether given degree of freedom is bounded
bool isBounded (size_type rank) const;
/// Get lower bound of given degree of freedom
double lowerBound (size_type rank) const;
value_type lowerBound (size_type rank) const;
/// Get upper bound of given degree of freedom
double upperBound (size_type rank) const;
value_type upperBound (size_type rank) const;
/// Set lower bound of given degree of freedom
void lowerBound (size_type rank, double lowerBound);
void lowerBound (size_type rank, value_type lowerBound);
/// Set upper bound of given degree of freedom
void upperBound (size_type rank, double upperBound);
void upperBound (size_type rank, value_type upperBound);
/// @}
private:
......@@ -146,7 +146,7 @@ namespace hpp {
virtual ~AnchorJointConfig ();
virtual void interpolate (ConfigurationIn_t,
ConfigurationIn_t,
const double&,
const value_type&,
const size_type&,
ConfigurationOut_t);
......@@ -155,9 +155,9 @@ namespace hpp {
/// \param index index of first component of q1 and q2 corresponding to
/// the joint.
/// \return 0
virtual double distance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const;
virtual value_type distance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const;
virtual void integrate (ConfigurationIn_t q,
vectorIn_t v,
......@@ -181,7 +181,7 @@ namespace hpp {
virtual ~SO3JointConfig ();
virtual void interpolate (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const double& u,
const value_type& u,
const size_type& index,
ConfigurationOut_t result);
......@@ -190,9 +190,9 @@ namespace hpp {
/// \param index index of first component of q1 and q2 corresponding to
/// the joint.
/// \return the angle between the joint orientations
virtual double distance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const;
virtual value_type distance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const;
virtual void integrate (ConfigurationIn_t q,
vectorIn_t v,
const size_type& indexConfig,
......@@ -237,7 +237,7 @@ namespace hpp {
virtual ~RotationJointConfig ();
virtual void interpolate (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const double& u,
const value_type& u,
const size_type& index,
ConfigurationOut_t result);
......@@ -246,9 +246,9 @@ namespace hpp {
/// \param index index of first component of q1 and q2 corresponding to
/// the joint.
/// \return the angle between the joint orientations
virtual double distance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const;
virtual value_type distance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const;
virtual void integrate (ConfigurationIn_t q,
vectorIn_t v,
const size_type& indexConfig,
......@@ -298,7 +298,7 @@ namespace hpp {
virtual ~TranslationJointConfig ();
virtual void interpolate (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const double& u,
const value_type& u,
const size_type& index,
ConfigurationOut_t result);
......@@ -307,9 +307,9 @@ namespace hpp {
/// \param index index of first component of q1 and q2 corresponding to
/// the joint.
/// \return the absolute value of the joint value difference.
virtual double distance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const;
virtual value_type distance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const;
virtual void integrate (ConfigurationIn_t q,
vectorIn_t v,
......
......@@ -168,13 +168,13 @@ namespace hpp {
/// Get whether given degree of freedom is bounded
bool isBounded (size_type rank) const;
/// Get lower bound of given degree of freedom
double lowerBound (size_type rank) const;
value_type lowerBound (size_type rank) const;
/// Get upper bound of given degree of freedom
double upperBound (size_type rank) const;
value_type upperBound (size_type rank) const;
/// Set lower bound of given degree of freedom
void lowerBound (size_type rank, double lowerBound);
void lowerBound (size_type rank, value_type lowerBound);
/// Set upper bound of given degree of freedom
void upperBound (size_type rank, double upperBound);
void upperBound (size_type rank, value_type upperBound);
/// \}
/// \name Jacobian
......@@ -215,7 +215,7 @@ namespace hpp {
Transform3f positionInParentFrame_;
mutable Transform3f T3f_;
/// Mass of this and all descendants
double mass_;
value_type mass_;
/// Mass time center of mass of this and all descendants
fcl::Vec3f massCom_;
private:
......@@ -236,14 +236,14 @@ namespace hpp {
virtual void writeSubJacobian (const JointPtr_t& child) = 0;
void computeJacobian ();
/// Compute mass of this and all descendants
double computeMass ();
value_type computeMass ();
/// Compute the product m * com
///
/// \li m is the mass of the joint and all descendants,
/// \li com is the center of mass of the joint and all descendants.
void computeMassTimesCenterOfMass ();
virtual void writeComSubjacobian (ComJacobian_t& jacobian,
const double& totalMass) = 0;
const value_type& totalMass) = 0;
size_type configSize_;
size_type numberDof_;
Transform3f initialPosition_;
......@@ -285,7 +285,7 @@ namespace hpp {
private:
virtual void writeSubJacobian (const JointPtr_t& child);
virtual void writeComSubjacobian (ComJacobian_t& jacobian,
const double& totalMass);
const value_type& totalMass);
}; // class JointAnchor
/// Spherical Joint
......@@ -310,7 +310,7 @@ namespace hpp {
private:
virtual void writeSubJacobian (const JointPtr_t& child);
virtual void writeComSubjacobian (ComJacobian_t& jacobian,
const double& totalMass);
const value_type& totalMass);
mutable fcl::Vec3f com_;
}; // class JointSO3
......@@ -337,9 +337,9 @@ namespace hpp {
private:
virtual void writeSubJacobian (const JointPtr_t& child);
virtual void writeComSubjacobian (ComJacobian_t& jacobian,
const double& totalMass);
const value_type& totalMass);
mutable fcl::Matrix3f R_;
mutable double angle_;
mutable value_type angle_;
mutable fcl::Vec3f axis_;
mutable fcl::Vec3f O2O1_;
mutable fcl::Vec3f cross_;
......@@ -369,7 +369,7 @@ namespace hpp {
private:
virtual void writeSubJacobian (const JointPtr_t& child);
virtual void writeComSubjacobian (ComJacobian_t& jacobian,
const double& totalMass);
const value_type& totalMass);
mutable fcl::Vec3f t_;
mutable fcl::Vec3f axis_ [dimension];
mutable fcl::Vec3f com_;
......
......@@ -28,7 +28,7 @@
namespace hpp {
namespace model {
typedef Eigen::AngleAxis <double> AngleAxis_t;
typedef Eigen::AngleAxis <value_type> AngleAxis_t;
typedef fcl::Quaternion3f Quaternion_t;
JointConfiguration::JointConfiguration (size_type numberDof)
......@@ -55,22 +55,22 @@ namespace hpp {
return bounded_ [rank];
}
double JointConfiguration::lowerBound (size_type rank) const
value_type JointConfiguration::lowerBound (size_type rank) const
{
return lowerBounds_ [rank];
}
double JointConfiguration::upperBound (size_type rank) const
value_type JointConfiguration::upperBound (size_type rank) const
{
return upperBounds_ [rank];
}
void JointConfiguration::lowerBound (size_type rank, double lowerBound)
void JointConfiguration::lowerBound (size_type rank, value_type lowerBound)
{
lowerBounds_ [rank] = lowerBound;
}
void JointConfiguration::upperBound (size_type rank, double upperBound)
void JointConfiguration::upperBound (size_type rank, value_type upperBound)
{
upperBounds_ [rank] = upperBound;
}
......@@ -112,15 +112,15 @@ namespace hpp {
void AnchorJointConfig::interpolate (ConfigurationIn_t,
ConfigurationIn_t,
const double&,
const value_type&,
const size_type&,
ConfigurationOut_t)
{
}
double AnchorJointConfig::distance (ConfigurationIn_t,
ConfigurationIn_t,
const size_type&) const
value_type AnchorJointConfig::distance (ConfigurationIn_t,
ConfigurationIn_t,
const size_type&) const
{
return 0;
}
......@@ -152,26 +152,26 @@ namespace hpp {
/// \param index index of joint configuration in robot configuration vector
/// \param unit quaternion corresponding to both joint configuration
/// \return angle between both joint configuration
static double angleBetweenQuaternions (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index)
static value_type angleBetweenQuaternions (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index)
{
double innerprod = q1.segment (index, 4).dot (q2.segment (index, 4));
value_type innerprod = q1.segment (index, 4).dot (q2.segment (index, 4));
assert (fabs (innerprod) < 1.0001);
if (innerprod < -1) innerprod = -1;
if (innerprod > 1) innerprod = 1;
double theta = acos (innerprod);
value_type theta = acos (innerprod);
return theta;
}
void SO3JointConfig::interpolate (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const double& u,
const value_type& u,
const size_type& index,
ConfigurationOut_t result)
{
// for rotation part, transform roll pitch yaw into quaternion
double theta = angleBetweenQuaternions (q1, q2, index);
value_type theta = angleBetweenQuaternions (q1, q2, index);
if (fabs (theta) > 1e-6) {
result.segment (index, 4) =
......@@ -183,11 +183,11 @@ namespace hpp {
}
}
double SO3JointConfig::distance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const
value_type SO3JointConfig::distance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const
{
double theta = angleBetweenQuaternions (q1, q2, index);
value_type theta = angleBetweenQuaternions (q1, q2, index);
assert (theta >= 0);
return theta;
}
......@@ -201,7 +201,7 @@ namespace hpp {
vector3_t omega (v [indexVelocity + 0], v [indexVelocity + 1],
v [indexVelocity + 2]);
double angle = .5*omega.norm();
value_type angle = .5*omega.norm();
if (angle == 0) {
result.segment (indexConfig, 4) = q.segment (indexConfig, 4);
return;
......@@ -234,7 +234,7 @@ namespace hpp {
Quaternion_t p2 (q2 [indexConfig + 0], q2 [indexConfig + 1],
q2 [indexConfig + 2], q2 [indexConfig + 3]);
Quaternion_t p (p1); p.conj (); p*=p2;
double angle; fcl::Vec3f axis;
value_type angle; fcl::Vec3f axis;
p.toAxisAngle (axis, angle);
result [indexVelocity + 0] = angle*axis [0];
result [indexVelocity + 1] = angle*axis [1];
......@@ -244,9 +244,9 @@ namespace hpp {
void SO3JointConfig::uniformlySample (const size_type& index,
ConfigurationOut_t result) const
{
double u1 = (double)rand() / RAND_MAX;
double u2 = (double)rand() / RAND_MAX;
double u3 = (double)rand() / RAND_MAX;
value_type u1 = (value_type)rand() / RAND_MAX;
value_type u2 = (value_type)rand() / RAND_MAX;
value_type u3 = (value_type)rand() / RAND_MAX;
result [index] = sqrt (1-u1)*sin(2*M_PI*u2);
result [index+1] = sqrt (1-u1)*cos(2*M_PI*u2);
result [index+2] = sqrt (u1) * sin(2*M_PI*u3);
......@@ -255,7 +255,7 @@ namespace hpp {
template <size_type dimension>
void TranslationJointConfig <dimension>::interpolate
(ConfigurationIn_t q1, ConfigurationIn_t q2, const double& u,
(ConfigurationIn_t q1, ConfigurationIn_t q2, const value_type& u,
const size_type& index, ConfigurationOut_t result)
{
result.segment <dimension> (index) =
......@@ -264,7 +264,7 @@ namespace hpp {
}
template <size_type dimension>
double TranslationJointConfig <dimension>::distance
value_type TranslationJointConfig <dimension>::distance
(ConfigurationIn_t q1, ConfigurationIn_t q2, const size_type& index) const
{
if (dimension == 1) {
......@@ -326,7 +326,7 @@ namespace hpp {
void RotationJointConfig::interpolate (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const double& u,
const value_type& u,
const size_type& index,
ConfigurationOut_t result)
{
......@@ -341,9 +341,9 @@ namespace hpp {
}
}
double RotationJointConfig::distance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const
value_type RotationJointConfig::distance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const
{
if (isBounded (0)) {
// linearly interpolate
......@@ -363,7 +363,7 @@ namespace hpp {
ConfigurationOut_t result) const
{
using jrlMathTools::Angle;
double omega = v [indexVelocity];
value_type omega = v [indexVelocity];
result [indexConfig] = Angle (q [indexConfig]) + Angle (omega);
if (isBounded (0)) {
if (result [indexConfig] < lowerBound (0)) {
......
......@@ -107,22 +107,22 @@ namespace hpp {
return configuration_->isBounded (rank);
}
double Joint::lowerBound (size_type rank) const
value_type Joint::lowerBound (size_type rank) const
{
return configuration_->lowerBound (rank);
}
double Joint::upperBound (size_type rank) const
value_type Joint::upperBound (size_type rank) const
{
return configuration_->upperBound (rank);
}
void Joint::lowerBound (size_type rank, double lower)
void Joint::lowerBound (size_type rank, value_type lower)
{
configuration_->lowerBound (rank, lower);
}
void Joint::upperBound (size_type rank, double upper)
void Joint::upperBound (size_type rank, value_type upper)
{
configuration_->upperBound (rank, upper);
}
......@@ -162,7 +162,7 @@ namespace hpp {
}
}
double Joint::computeMass ()
value_type Joint::computeMass ()
{
mass_ = 0;
if (body_) {
......@@ -222,7 +222,7 @@ namespace hpp {
}
void JointAnchor::writeComSubjacobian (ComJacobian_t&,
const double&)
const value_type&)
{
}
......@@ -286,7 +286,7 @@ namespace hpp {
}
void JointSO3::writeComSubjacobian (ComJacobian_t& jacobian,
const double& totalMass)
const value_type& totalMass)
{
if (mass_ > 0) {
const fcl::Vec3f& center (currentTransformation_.getTranslation ());
......@@ -347,7 +347,7 @@ namespace hpp {
}
void JointRotation::writeComSubjacobian (ComJacobian_t& jacobian,
const double& totalMass)
const value_type& totalMass)
{
if (mass_ > 0) {
size_type col = rankInVelocity ();
......@@ -489,7 +489,7 @@ namespace hpp {
template <size_type dimension>
void JointTranslation <dimension>::writeComSubjacobian
(ComJacobian_t& jacobian, const double& totalMass)
(ComJacobian_t& jacobian, const value_type& totalMass)
{
if (mass_ > 0) {
size_type col = rankInVelocity ();
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment