Commit 97b351e4 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Split rotation joint into 2 joints

  - jointRotation::Bounded and UnBounded.
parent 2df38b95
......@@ -65,7 +65,6 @@ SET(${PROJECT_NAME}_HEADERS
SEARCH_FOR_BOOST()
ADD_REQUIRED_DEPENDENCY("eigen3 >= 3.2")
ADD_REQUIRED_DEPENDENCY("jrl-mathtools >= 1.5")
ADD_REQUIRED_DEPENDENCY("hpp-util >= 0.7")
ADD_REQUIRED_DEPENDENCY("fcl >= 0.2.9")
......
......@@ -97,7 +97,7 @@ namespace hpp {
JointPtr_t rootJoint () const;
/// Register joint in internal containers
void registerJoint (JointPtr_t joint);
void registerJoint (const JointPtr_t& joint);
/// Get vector of joints
const JointVector_t& getJointVector () const;
......@@ -149,7 +149,7 @@ namespace hpp {
void setDimensionExtraConfigSpace (const size_type& dimension)
{
extraConfigSpace_.setDimension (dimension);
resizeState ();
resizeState (0x0);
}
///
......@@ -328,7 +328,7 @@ namespace hpp {
void computeMass ();
void computePositionCenterOfMass ();
void computeJacobianCenterOfMass ();
void resizeState ();
void resizeState (const JointPtr_t& joint);
void resizeJacobians ();
std::string name_;
DistanceResults_t distances_;
......
......@@ -232,13 +232,16 @@ namespace hpp {
class HPP_MODEL_DLLAPI RotationJointConfig : public JointConfiguration
{
public:
RotationJointConfig ();
/// Constructor
/// \param configSize dimension of the joint configuration size: used to
/// resize the vector of bounds.
RotationJointConfig (size_type configSize);
virtual ~RotationJointConfig ();
virtual void interpolate (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const value_type& u,
const size_type& index,
ConfigurationOut_t result);
ConfigurationOut_t result) = 0;
/// Distance between two configurations of the joint
/// \param q1, q2 two configurations of the robot
......@@ -247,12 +250,12 @@ namespace hpp {
/// \return the angle between the joint orientations
virtual value_type distance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const;
const size_type& index) const = 0;
virtual void integrate (ConfigurationIn_t q,
vectorIn_t v,
const size_type& indexConfig,
const size_type& indexVelocity,
ConfigurationOut_t result) const;
ConfigurationOut_t result) const = 0;
/// Difference between two configurations
///
......@@ -282,12 +285,62 @@ namespace hpp {
ConfigurationIn_t q2,
const size_type& indexConfig,
const size_type& indexVelocity,
vectorOut_t result) const;
vectorOut_t result) const = 0;
virtual void uniformlySample (const size_type& index,
ConfigurationOut_t result) const;
ConfigurationOut_t result) const = 0;
}; // class RotationJointConfig
namespace rotationJointConfig {
class HPP_MODEL_DLLAPI UnBounded : public JointConfiguration
{
public:
UnBounded ();
virtual ~UnBounded ()
{
}
void interpolate (ConfigurationIn_t q1, ConfigurationIn_t q2,
const value_type& u, const size_type& index,
ConfigurationOut_t result);
value_type distance (ConfigurationIn_t q1, ConfigurationIn_t q2,
const size_type& index) const;
void integrate (ConfigurationIn_t q, vectorIn_t v,
const size_type& indexConfig,
const size_type& indexVelocity,
ConfigurationOut_t result) const;
void difference (ConfigurationIn_t q1, ConfigurationIn_t q2,
const size_type& indexConfig,
const size_type& indexVelocity,
vectorOut_t result) const;
void uniformlySample (const size_type& index,
ConfigurationOut_t result) const;
}; // class UnBounded
class HPP_MODEL_DLLAPI Bounded : public JointConfiguration
{
public:
Bounded ();
virtual ~Bounded ()
{
}
void interpolate (ConfigurationIn_t q1, ConfigurationIn_t q2,
const value_type& u, const size_type& index,
ConfigurationOut_t result);
value_type distance (ConfigurationIn_t q1, ConfigurationIn_t q2,
const size_type& index) const;
void integrate (ConfigurationIn_t q, vectorIn_t v,
const size_type& indexConfig,
const size_type& indexVelocity,
ConfigurationOut_t result) const;
void difference (ConfigurationIn_t q1, ConfigurationIn_t q2,
const size_type& indexConfig,
const size_type& indexVelocity,
vectorOut_t result) const;
void uniformlySample (const size_type& index,
ConfigurationOut_t result) const;
}; // class Bounded
} // namespace rotationJointConfig
/// Configuration of a JointTranslation
template <size_type dimension>
class HPP_MODEL_DLLAPI TranslationJointConfig : public JointConfiguration
......
......@@ -99,6 +99,17 @@ namespace hpp {
const Transform3f& parentPosition,
Transform3f& position) const = 0;
/// Get neutral configuration of joint
///
/// Neutral configuration is
/// \li 0 for translation joints,
/// \li (1,0,0,0) for SO3 joints,
/// \li (1,0) for unbounded rotation joint
/// \li 0 for bounded rotation joint.
vector_t neutralConfiguration () const
{
return neutralConfiguration_;
}
///\}
/// Return number of degrees of freedom
......@@ -245,6 +256,7 @@ namespace hpp {
/// Mass time center of mass of this and all descendants
fcl::Vec3f massCom_;
value_type maximalDistanceToParent_;
vector_t neutralConfiguration_;
private:
/// Compute position of this joint and all its descendents.
void recursiveComputePosition (ConfigurationIn_t configuration,
......@@ -377,18 +389,24 @@ namespace hpp {
class HPP_MODEL_DLLAPI JointRotation : public Joint
{
public:
JointRotation (const Transform3f& initialPosition);
/// Constructor
/// \param initialPosition position of the joint in global frame before
/// being linked to a parent,
/// \param configSize dimension of the configuration vector,
/// \param numberDof dimension of the velocity vector.
JointRotation (const Transform3f& initialPosition, size_type configSize,
size_type numberDof);
JointRotation (const JointRotation& joint);
/// Return pointer to copy of this
/// Clone body and therefore inner and outer objects (see Body::clone).
virtual JointPtr_t clone () const;
virtual JointPtr_t clone () const = 0;
/// Compute position of joint
/// \param configuration the configuration of the robot,
/// \param parentPosition position of parent joint,
/// \retval position position of this joint.
virtual void computePosition (ConfigurationIn_t configuration,
const Transform3f& parentPosition,
Transform3f& position) const;
Transform3f& position) const = 0;
virtual ~JointRotation ();
/// Get upper bound on linear velocity of the joint frame
/// \return 0
......@@ -404,18 +422,71 @@ namespace hpp {
}
protected:
virtual void computeMaximalDistanceToParent ();
private:
virtual void writeSubJacobian (const JointPtr_t& child);
virtual void writeComSubjacobian (ComJacobian_t& jacobian,
const value_type& totalMass);
mutable fcl::Matrix3f R_;
mutable value_type angle_;
mutable fcl::Vec3f axis_;
mutable fcl::Vec3f O2O1_;
mutable fcl::Vec3f cross_;
mutable fcl::Vec3f com_;
private:
virtual void writeSubJacobian (const JointPtr_t& child);
virtual void writeComSubjacobian (ComJacobian_t& jacobian,
const value_type& totalMass);
}; // class JointRotation
namespace jointRotation {
/// Rotation about an axis without bound
///
/// The configuration space of this joint is the unit circle, represented
/// by \f$(q_0, q_1)\f$ such that \f$q_0^2 + q_1^2=1\f$
class HPP_MODEL_DLLAPI UnBounded : public JointRotation
{
public:
UnBounded (const Transform3f& initialPosition);
UnBounded (const UnBounded& joint);
/// Return pointer to copy of this
/// Clone body and therefore inner and outer objects (see Body::clone).
JointPtr_t clone () const;
/// Compute position of joint
/// \param configuration the configuration of the robot,
/// \param parentPosition position of parent joint,
/// \retval position position of this joint.
virtual void computePosition (ConfigurationIn_t configuration,
const Transform3f& parentPosition,
Transform3f& position) const;
virtual ~UnBounded ()
{
}
private:
mutable value_type cosAngle_;
mutable value_type sinAngle_;
}; // class UnBounded
/// Rotation about an axis with bound
///
/// The configuration space of this joint is an interval of angle.
class HPP_MODEL_DLLAPI Bounded : public JointRotation
{
public:
Bounded (const Transform3f& initialPosition);
Bounded (const Bounded& joint);
/// Return pointer to copy of this
/// Clone body and therefore inner and outer objects (see Body::clone).
JointPtr_t clone () const;
/// Compute position of joint
/// \param configuration the configuration of the robot,
/// \param parentPosition position of parent joint,
/// \retval position position of this joint.
virtual void computePosition (ConfigurationIn_t configuration,
const Transform3f& parentPosition,
Transform3f& position) const;
virtual ~Bounded ()
{
}
private:
mutable value_type angle_;
}; // class Bounded
} // namespace jointRotation
/// Translation Joint
///
/// Map a 1,2 or 3-dimensional input vector to a translation
......
......@@ -53,10 +53,15 @@ namespace hpp {
{
return new JointAnchor (initialPosition);
}
virtual JointPtr_t createJointRotation
virtual JointPtr_t createBoundedJointRotation
(const Transform3f& initialPosition)
{
return new JointRotation (initialPosition);
return new jointRotation::Bounded (initialPosition);
}
virtual JointPtr_t createUnBoundedJointRotation
(const Transform3f& initialPosition)
{
return new jointRotation::UnBounded (initialPosition);
}
virtual JointPtr_t createJointTranslation
(const Transform3f& initialPosition)
......
......@@ -33,7 +33,6 @@ ADD_LIBRARY(${LIBRARY_NAME}
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} hpp-util)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} fcl)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} jrl-mathtools)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} eigen3)
INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION lib)
......@@ -409,27 +409,33 @@ namespace hpp {
// ========================================================================
void Device::registerJoint (JointPtr_t joint)
void Device::registerJoint (const JointPtr_t& joint)
{
jointVector_.push_back (joint);
joint->rankInConfiguration_ = configSize_;
joint->rankInVelocity_ = numberDof_;
numberDof_ += joint->numberDof ();
configSize_ += joint->configSize ();
resizeState ();
resizeState (joint);
jointByName_ [joint->name ()] = joint;
resizeJacobians ();
computeMass ();
}
void Device::resizeState ()
void Device::resizeState (const JointPtr_t& joint)
{
size_type oldSize = currentConfiguration_.size ();
size_type newSize = configSize ();
Configuration_t q = currentConfiguration_;
currentConfiguration_.resize (newSize);
// if size of configuration increased, set last coordinates to 0
if (newSize > oldSize) {
currentConfiguration_.head (oldSize) = q;
currentConfiguration_.tail (newSize - oldSize).setZero ();
if (joint) {
currentConfiguration_.tail (newSize - oldSize) =
joint->neutralConfiguration ();
}
}
oldSize = currentVelocity_.size ();
newSize = numberDof ();
......
......@@ -23,7 +23,6 @@
#include <iostream>
#include <sstream>
#include <fcl/math/transform.h>
#include <jrl/mathtools/angle.hh>
#include <hpp/util/debug.hh>
#include <hpp/model/joint-configuration.hh>
......@@ -98,16 +97,6 @@ namespace hpp {
{
}
RotationJointConfig::RotationJointConfig () : JointConfiguration (1)
{
lowerBound (0, -M_PI);
upperBound (0, M_PI);
}
RotationJointConfig::~RotationJointConfig ()
{
}
template <size_type dimension>
TranslationJointConfig <dimension>::TranslationJointConfig () :
JointConfiguration (dimension)
......@@ -333,85 +322,137 @@ namespace hpp {
}
}
void RotationJointConfig::interpolate (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const value_type& u,
const size_type& index,
ConfigurationOut_t result)
{
if (isBounded (0)) {
template class TranslationJointConfig <1>;
template class TranslationJointConfig <2>;
template class TranslationJointConfig <3>;
namespace rotationJointConfig {
void UnBounded::interpolate (ConfigurationIn_t q1, ConfigurationIn_t q2,
const value_type& u, const size_type& index,
ConfigurationOut_t result)
{
// interpolate on the unit circle
double c1 = q1 [index], s1 = q1 [index + 1];
double c2 = q2 [index], s2 = q2 [index + 1];
double cosTheta = c1*c2 + s1*s2;
double sinTheta = c1*s2 - s1*c2;
double theta = atan2 (sinTheta, cosTheta);
assert (fabs (sin (theta) - sinTheta) < 1e-8);
if (fabs (theta) > 1e-6) {
result.segment (index, 2) =
(sin ((1-u)*theta)/sinTheta) * q1.segment (index, 2) +
(sin (u*theta)/sinTheta) * q2.segment (index, 2);
} else {
result.segment (index, 2) =
(1-u) * q1.segment (index, 2) + u * q2.segment (index, 2);
}
}
value_type UnBounded::distance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const
{
// distance on the unit circle
value_type innerprod =
q1.segment (index, 2).dot (q2.segment (index, 2));
assert (fabs (innerprod) < 1.0001);
if (innerprod < -1) innerprod = -1;
if (innerprod > 1) innerprod = 1;
value_type theta = acos (innerprod);
return theta;
}
void UnBounded::integrate (ConfigurationIn_t q, vectorIn_t v,
const size_type& indexConfig,
const size_type& indexVelocity,
ConfigurationOut_t result) const
{
value_type omega = v [indexVelocity];
value_type cosOmega = cos (omega);
value_type sinOmega = sin (omega);
value_type norm2p = q.segment (indexConfig, 2).squaredNorm ();
result [indexConfig] = (1.5-.5*norm2p) *
(cosOmega * q [indexConfig] - sinOmega * q [indexConfig + 1]);
result [indexConfig + 1] = (1.5-.5*norm2p) *
sinOmega * q [indexConfig] + cosOmega * q [indexConfig + 1];
}
void UnBounded::difference (ConfigurationIn_t q1, ConfigurationIn_t q2,
const size_type& indexConfig,
const size_type& indexVelocity,
vectorOut_t result) const
{
value_type c1 = q1 [indexConfig];
value_type s1 = q1 [indexConfig + 1];
value_type c2 = q2 [indexConfig];
value_type s2 = q2 [indexConfig + 1];
result [indexVelocity] = atan2 (-s1*c2 + s2*c1, c1*c2 + s1*s2);
}
void UnBounded::uniformlySample (const size_type& index,
ConfigurationOut_t result) const
{
value_type angle = -M_PI + 2* M_PI * rand ()/RAND_MAX;
result [index] = cos (angle);
result [index + 1] = sin (angle);
}
UnBounded::UnBounded () : JointConfiguration (2)
{
}
void Bounded::interpolate (ConfigurationIn_t q1, ConfigurationIn_t q2,
const value_type& u, const size_type& index,
ConfigurationOut_t result)
{
// linearly interpolate
result [index] = (1-u) * q1 [index] + u * q2 [index];
} else {
// interpolate on the unit circle
jrlMathTools::Angle th1 (q1 [index]);
jrlMathTools::Angle th2 (q2 [index]);
result [index] = th1.interpolate (u, th2);
}
}
value_type RotationJointConfig::distance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const
{
if (isBounded (0)) {
value_type Bounded::distance (ConfigurationIn_t q1, ConfigurationIn_t q2,
const size_type& index) const
{
// linearly interpolate
return fabs (q2 [index] - q1 [index]);
} else {
// distance on the unit circle
jrlMathTools::Angle th1 (q1 [index]);
jrlMathTools::Angle th2 (q2 [index]);
return th1.distance (th2);
}
}
void RotationJointConfig::integrate (ConfigurationIn_t q,
vectorIn_t v,
const size_type& indexConfig,
const size_type& indexVelocity,
ConfigurationOut_t result) const
{
using jrlMathTools::Angle;
value_type omega = v [indexVelocity];
result [indexConfig] = Angle (q [indexConfig]) + Angle (omega);
if (isBounded (0)) {
void Bounded::integrate (ConfigurationIn_t q, vectorIn_t v,
const size_type& indexConfig,
const size_type& indexVelocity,
ConfigurationOut_t result) const
{
value_type omega = v [indexVelocity];
result [indexConfig] = q [indexConfig] + omega;
if (result [indexConfig] < lowerBound (0)) {
result [indexConfig] = lowerBound (0);
} else if (result [indexConfig] > upperBound (0)) {
result [indexConfig] = upperBound (0);
}
}
}
void RotationJointConfig::difference (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& indexConfig,
const size_type& indexVelocity,
vectorOut_t result) const
{
using jrlMathTools::Angle;
if (isBounded (0)) {
result [indexVelocity] = q1 [indexConfig] - q2 [indexConfig];
} else {
result [indexVelocity] = Angle (q1 [indexVelocity]) -
Angle (q2 [indexVelocity]);
void Bounded::difference (ConfigurationIn_t q1, ConfigurationIn_t q2,
const size_type& indexConfig,
const size_type& indexVelocity,
vectorOut_t result) const
{
result [indexVelocity] = q1 [indexConfig] - q2 [indexConfig];
}
}
void RotationJointConfig::uniformlySample (const size_type& index,
ConfigurationOut_t result) const
{
if (!isBounded (0)) {
result [index] = -M_PI + 2* M_PI * rand ()/RAND_MAX;
}
else {
void Bounded::uniformlySample (const size_type& index,
ConfigurationOut_t result) const
{
result [index] = lowerBound (0) +
(upperBound (0) - lowerBound (0)) * rand ()/RAND_MAX;
}
}
template class TranslationJointConfig <1>;
template class TranslationJointConfig <2>;
template class TranslationJointConfig <3>;
Bounded::Bounded () : JointConfiguration (1)
{
isBounded (0, true);
lowerBound (0, -M_PI);
upperBound (0, M_PI);
}
} // namespace rotationJointConfig
} // namespace model
} // namespace hpp
......@@ -45,6 +45,8 @@ namespace hpp {
positionInParentFrame_.setIdentity ();
T3f_.setIdentity ();
massCom_.setValue (0);
neutralConfiguration_.resize (configSize);
neutralConfiguration_.setZero ();
}
Joint::Joint (const Joint& joint) :
......@@ -242,6 +244,7 @@ namespace hpp {
Joint (initialPosition, 4, 3)
{
configuration_ = new SO3JointConfig;
neutralConfiguration_ [0] = 1;
}
JointSO3::JointSO3 (const JointSO3& joint) :
......@@ -314,10 +317,10 @@ namespace hpp {
}
}
JointRotation::JointRotation (const Transform3f& initialPosition) :
Joint (initialPosition, 1, 1), R_ ()
JointRotation::JointRotation (const Transform3f& initialPosition,
size_type configSize, size_type numberDof) :
Joint (initialPosition, configSize, numberDof), R_ ()
{
configuration_ = new RotationJointConfig;
R_.setIdentity ();
}
......@@ -327,11 +330,6 @@ namespace hpp {
R_.setIdentity ();
}
JointPtr_t JointRotation::clone () const
{
return new JointRotation (*this);
}
JointRotation::~JointRotation ()
{
delete configuration_;
......@@ -343,17 +341,6 @@ namespace hpp {
positionInParentFrame ().getTranslation ().length ();
}
void JointRotation::computePosition (ConfigurationIn_t configuration,
const Transform3f& parentPosition,
Transform3f& position) const
{
angle_ = configuration [rankInConfiguration ()];
R_ (1,1) = cos (angle_); R_ (1,2) = -sin (angle_);
R_ (2,1) = sin (angle_); R_ (2,2) = cos (angle_);
T3f_.setRotation (R_);
position = parentPosition * positionInParentFrame_ * T3f_;
}
void JointRotation::writeSubJacobian (const JointPtr_t& child)
{
size_type col = rankInVelocity ();
......@@ -386,6 +373,63 @@ namespace hpp {
}
}