Commit 769cabc1 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Create 3 types of translation joints

  - JointTranslation is now a template class with size_type as a template	    argument. The template argument represent the dimension of the joint
    configuration.
parent 61012ddd
......@@ -40,12 +40,29 @@ namespace hpp {
HPP_PREDEF_CLASS (JointAnchor);
HPP_PREDEF_CLASS (JointRotation);
HPP_PREDEF_CLASS (JointSO3);
HPP_PREDEF_CLASS (JointTranslation);
HPP_PREDEF_CLASS (JointConfiguration);
HPP_PREDEF_CLASS (ObjectFactory);
HPP_PREDEF_CLASS (ObjectIterator);
HPP_PREDEF_CLASS (Gripper);
enum Request_t {COLLISION, DISTANCE};
typedef double value_type;
typedef Eigen::Matrix <double, 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::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::Block <JointJacobian_t, 3, Eigen::Dynamic>
HalfJointJacobian_t;
typedef Body* BodyPtr_t;
typedef std::vector<Body*> BodyVector_t;
typedef boost::shared_ptr <CollisionObject> CollisionObjectPtr_t;
......@@ -58,32 +75,21 @@ namespace hpp {
typedef JointAnchor* JointAnchorPtr_t;
typedef JointRotation* JointRotationPtr_t;
typedef JointSO3* JointSO3Ptr_t;
typedef JointTranslation* JointTranslationPtr_t;
template <size_type dimension = 1> class JointTranslation;
typedef JointTranslation <1>* JointTranslationPtr_t;
typedef JointTranslation <2>* JointTranslation2Ptr_t;
typedef JointTranslation <3>* JointTranslation3Ptr_t;
typedef const Joint* JointConstPtr_t;
typedef const JointAnchor* JointAnchorConstPtr_t;
typedef const JointRotation* JointRotationConstPtr_t;
typedef const JointSO3* JointSO3ConstPtr_t;
typedef const JointTranslation * JointTranslationConstPtr_t;
typedef const JointTranslation <1>* JointTranslationConstPtr_t;
typedef const JointTranslation <2>* JointTranslation2ConstPtr_t;
typedef const JointTranslation <3>* JointTranslation3ConstPtr_t;
typedef std::map <std::string, JointPtr_t> JointByName_t;
typedef std::vector <JointPtr_t> JointVector_t;
typedef std::vector <JointPtr_t> JointVector_t;
typedef boost::shared_ptr <Gripper> GripperPtr_t;
typedef std::vector <GripperPtr_t> Grippers_t;
typedef double value_type;
typedef Eigen::Matrix <double, 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::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::Block <JointJacobian_t, 3, Eigen::Dynamic>
HalfJointJacobian_t;
typedef fcl::Transform3f Transform3f;
} // namespace model
} // namespace hpp
......
......@@ -290,6 +290,7 @@ namespace hpp {
}; // class RotationJointConfig
/// Configuration of a JointTranslation
template <size_type dimension>
class HPP_MODEL_DLLAPI TranslationJointConfig : public JointConfiguration
{
public:
......
......@@ -31,7 +31,7 @@ namespace hpp {
///
/// A joint maps an input vector to a transformation of SE(3) from the
/// parent frame to the joint frame.
///
///
/// The input vector is provided through the configuration vector of the
/// robot the joint belongs to. The joint input vector is composed of the
/// components of the robot configuration starting at
......@@ -348,13 +348,13 @@ namespace hpp {
/// Translation Joint
///
/// Map a length as one-dimensional input vector to a translation along a
/// fixed axis in parent frame.
/// Map a 1,2 or 3-dimensional input vector to a translation
template <size_type dimension>
class HPP_MODEL_DLLAPI JointTranslation : public Joint
{
public:
JointTranslation (const Transform3f& initialPosition);
JointTranslation (const JointTranslation& joint);
JointTranslation (const JointTranslation <dimension>& joint);
/// Return pointer to copy of this
/// Clone body and therefore inner and outer objects (see Body::clone).
virtual JointPtr_t clone () const;
......@@ -371,7 +371,7 @@ namespace hpp {
virtual void writeComSubjacobian (ComJacobian_t& jacobian,
const double& totalMass);
mutable fcl::Vec3f t_;
mutable fcl::Vec3f axis_;
mutable fcl::Vec3f axis_ [dimension];
mutable fcl::Vec3f com_;
}; // class JointTranslation
......@@ -382,5 +382,4 @@ namespace fcl {
std::ostream& operator<< (std::ostream& os , const fcl::Transform3f& trans);
}
std::ostream& operator<< (std::ostream& os, const hpp::model::Joint& joint);
#endif // HPP_MODEL_JOINT_HH
......@@ -36,7 +36,7 @@ namespace hpp {
ObjectFactory ()
{}
virtual ~ObjectFactory () {}
virtual DevicePtr_t createRobot (const std::string& name)
{
return Device::create (name);
......@@ -61,7 +61,17 @@ namespace hpp {
virtual JointPtr_t createJointTranslation
(const Transform3f& initialPosition)
{
return new JointTranslation (initialPosition);
return new JointTranslation <1> (initialPosition);
}
virtual JointPtr_t createJointTranslation2
(const Transform3f& initialPosition)
{
return new JointTranslation <2> (initialPosition);
}
virtual JointPtr_t createJointTranslation3
(const Transform3f& initialPosition)
{
return new JointTranslation <3> (initialPosition);
}
virtual BodyPtr_t createBody ()
{
......
......@@ -99,12 +99,14 @@ namespace hpp {
{
}
TranslationJointConfig::TranslationJointConfig () :
JointConfiguration (1)
template <size_type dimension>
TranslationJointConfig <dimension>::TranslationJointConfig () :
JointConfiguration (dimension)
{
}
TranslationJointConfig::~TranslationJointConfig ()
template <size_type dimension>
TranslationJointConfig <dimension>::~TranslationJointConfig ()
{
}
......@@ -251,61 +253,74 @@ namespace hpp {
result [index+3] = sqrt (u1) * cos(2*M_PI*u3);
}
void TranslationJointConfig::interpolate (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const double& u,
const size_type& index,
ConfigurationOut_t result)
template <size_type dimension>
void TranslationJointConfig <dimension>::interpolate
(ConfigurationIn_t q1, ConfigurationIn_t q2, const double& u,
const size_type& index, ConfigurationOut_t result)
{
result [index] = (1-u) * q1 [index] + u * q2 [index];
result.segment <dimension> (index) =
(1-u) * q1.segment <dimension> (index) +
u * q2.segment <dimension> (index);
}
double TranslationJointConfig::distance (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& index) const
template <size_type dimension>
double TranslationJointConfig <dimension>::distance
(ConfigurationIn_t q1, ConfigurationIn_t q2, const size_type& index) const
{
return fabs (q2 [index] - q1 [index]);
if (dimension == 1) {
return fabs (q2 [index] - q1 [index]);
} else {
return (q2.segment <dimension> (index) -
q1.segment <dimension> (index)).norm ();
}
}
void TranslationJointConfig::integrate (ConfigurationIn_t q,
vectorIn_t v,
const size_type& indexConfig,
const size_type& indexVelocity,
ConfigurationOut_t result) const
template <size_type dimension>
void TranslationJointConfig <dimension>::integrate
(ConfigurationIn_t q, vectorIn_t v, const size_type& indexConfig,
const size_type& indexVelocity, ConfigurationOut_t result) const
{
assert (indexConfig < result.size ());
result [indexConfig] = q [indexConfig] + v [indexVelocity];
if (isBounded (0)) {
if (result [indexConfig] < lowerBound (0)) {
result [indexConfig] = lowerBound (0);
} else if (result [indexConfig] > upperBound (0)) {
result [indexConfig] = upperBound (0);
result.segment <dimension> (indexConfig) =
q.segment <dimension> (indexConfig) +
v.segment <dimension> (indexVelocity);
for (unsigned int i=0; i<dimension; ++i) {
if (isBounded (i)) {
if (result [indexConfig + i] < lowerBound (i)) {
result [indexConfig + i] = lowerBound (i);
} else if (result [indexConfig + i] > upperBound (i)) {
result [indexConfig + i] = upperBound (i);
}
}
}
}
void TranslationJointConfig::difference (ConfigurationIn_t q1,
ConfigurationIn_t q2,
const size_type& indexConfig,
const size_type& indexVelocity,
vectorOut_t result) const
template <size_type dimension>
void TranslationJointConfig <dimension>::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];
result.segment <dimension> (indexVelocity) =
q1.segment <dimension> (indexConfig) -
q2.segment <dimension> (indexConfig);
}
void TranslationJointConfig::uniformlySample (const size_type& index,
ConfigurationOut_t result) const
template <size_type dimension>
void TranslationJointConfig <dimension>::uniformlySample
(const size_type& index, ConfigurationOut_t result) const
{
if (!isBounded (0)) {
std::ostringstream iss
("Cannot uniformly sample non bounded translation degrees of "
"freedom at rank ");
iss << index;
throw std::runtime_error (iss.str ());
}
else {
result [index] = lowerBound (0) +
(upperBound (0) - lowerBound (0)) * rand ()/RAND_MAX;
for (unsigned int i=0; i<dimension; ++i) {
if (!isBounded (i)) {
std::ostringstream iss
("Cannot uniformly sample non bounded translation degrees of "
"freedom at rank ");
iss << index + i;
throw std::runtime_error (iss.str ());
}
else {
result [index + i] = lowerBound (i) +
(upperBound (i) - lowerBound (i)) * rand ()/RAND_MAX;
}
}
}
......@@ -385,6 +400,9 @@ namespace hpp {
(upperBound (0) - lowerBound (0)) * rand ()/RAND_MAX;
}
}
template class TranslationJointConfig <1>;
template class TranslationJointConfig <2>;
template class TranslationJointConfig <3>;
} // namespace model
} // namespace hpp
......@@ -362,60 +362,6 @@ namespace hpp {
}
}
JointTranslation::JointTranslation (const Transform3f& initialPosition) :
Joint (initialPosition, 1, 1), t_ ()
{
configuration_ = new TranslationJointConfig;
t_.setValue (0);
}
JointTranslation::JointTranslation (const JointTranslation& joint) :
Joint (joint), t_ ()
{
t_.setValue (0);
}
JointPtr_t JointTranslation::clone () const
{
return new JointTranslation (*this);
}
JointTranslation::~JointTranslation ()
{
delete configuration_;
}
void JointTranslation::computePosition
(ConfigurationIn_t configuration, const Transform3f& parentPosition,
Transform3f& position) const
{
t_ [0] = configuration [rankInConfiguration ()];
T3f_.setTranslation (t_);
position = parentPosition * positionInParentFrame_ * T3f_;
}
void JointTranslation::writeSubJacobian (const JointPtr_t& child)
{
size_type col = rankInVelocity ();
// Get translation axis
axis_ = currentTransformation_.getRotation ().getColumn (0);
child->jacobian () (0, col) = axis_ [0];
child->jacobian () (1, col) = axis_ [1];
child->jacobian () (2, col) = axis_ [2];
}
void JointTranslation::writeComSubjacobian (ComJacobian_t& jacobian,
const double& totalMass)
{
if (mass_ > 0) {
size_type col = rankInVelocity ();
// Get translation axis
axis_ = currentTransformation_.getRotation ().getColumn (0);
jacobian (0, col) = (mass_/totalMass) * axis_ [0];
jacobian (1, col) = (mass_/totalMass) * axis_ [1];
jacobian (2, col) = (mass_/totalMass) * axis_ [2];
}
}
std::ostream& displayTransform3f (std::ostream& os,
const fcl::Transform3f trans)
......@@ -479,7 +425,86 @@ namespace hpp {
}
return os;
}
template <size_type dimension>
JointTranslation <dimension>::JointTranslation
(const Transform3f& initialPosition) : Joint (initialPosition, dimension,
dimension), t_ ()
{
if (dimension > 3 || dimension ==0) {
throw std::runtime_error
("Dimension of translation should be between 1 and 3.");
}
configuration_ = new TranslationJointConfig <dimension>;
t_.setValue (0);
}
template <size_type dimension>
JointTranslation <dimension>::JointTranslation
(const JointTranslation <dimension>& joint) : Joint (joint), t_ ()
{
t_.setValue (0);
}
template <size_type dimension>
JointPtr_t JointTranslation <dimension>::clone () const
{
return new JointTranslation <dimension> (*this);
}
template <size_type dimension>
JointTranslation <dimension>::~JointTranslation ()
{
delete configuration_;
}
template <size_type dimension>
void JointTranslation <dimension>::computePosition
(ConfigurationIn_t configuration, const Transform3f& parentPosition,
Transform3f& position) const
{
t_ [0] = configuration [rankInConfiguration ()];
if (dimension >= 2) {
t_ [1] = configuration [rankInConfiguration () + 1];
}
if (dimension >= 3) {
t_ [2] = configuration [rankInConfiguration () + 2];
}
T3f_.setTranslation (t_);
position = parentPosition * positionInParentFrame_ * T3f_;
}
template <size_type dimension>
void JointTranslation <dimension>::writeSubJacobian
(const JointPtr_t& child)
{
size_type col = rankInVelocity ();
// Get translation axis
for (unsigned int i=0; i<dimension; ++i) {
axis_[i] = currentTransformation_.getRotation ().getColumn (i);
child->jacobian () (0, col+i) = axis_ [i][0];
child->jacobian () (1, col+i) = axis_ [i][1];
child->jacobian () (2, col+i) = axis_ [i][2];
}
}
template <size_type dimension>
void JointTranslation <dimension>::writeComSubjacobian
(ComJacobian_t& jacobian, const double& totalMass)
{
if (mass_ > 0) {
size_type col = rankInVelocity ();
// Get translation axis
for (unsigned int i=0; i<dimension; ++i) {
axis_ [i] = currentTransformation_.getRotation ().getColumn (i);
jacobian (0, col+i) = (mass_/totalMass) * axis_ [i][0];
jacobian (1, col+i) = (mass_/totalMass) * axis_ [i][1];
jacobian (2, col+i) = (mass_/totalMass) * axis_ [i][2];
}
}
}
template class JointTranslation <1>;
template class JointTranslation <2>;
template class JointTranslation <3>;
} // namespace model
} // namespace hpp
......@@ -502,3 +527,4 @@ std::ostream& operator<< (std::ostream& os, const hpp::model::Joint& joint)
{
return joint.display (os);
}
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