Commit 09eeeabe authored by Valenza Florian's avatar Valenza Florian
Browse files

[C++] Added JointAccessor + its unittests.

Completed the visitors on JointVariants for each method of the JointBase interface  and moved it to a different file.
parent aa06d835
......@@ -119,6 +119,9 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
multibody/joint/joint-free-flyer.hpp
multibody/joint/joint-variant.hpp
multibody/joint/joint-generic.hpp
multibody/joint/joint-accessor.hpp
multibody/joint/joint-basic-visitors.hpp
multibody/joint/joint-basic-visitors.hxx
)
SET(${PROJECT_NAME}_MULTIBODY_PARSER_HEADERS
......
......@@ -24,7 +24,14 @@ namespace se3
{
template<bool condition>
struct static_assertion {};
struct static_assertion
{
enum
{
THIS_METHOD_SHOULD_NOT_BE_CALLED_ON_DERIVED_CLASS,
MIXING_JOINT_MODEL_AND_DATA_OF_DIFFERENT_TYPE
};
};
template<>
struct static_assertion<true>
......
//
// Copyright (c) 2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio 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.
//
// Pinocchio 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
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#ifndef __se3_joint_accessor_hpp__
#define __se3_joint_accessor_hpp__
#include "pinocchio/assert.hpp"
#include "pinocchio/multibody/joint/joint-variant.hpp"
#include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
namespace se3
{
struct JointAccessor;
struct JointModelAccessor;
struct JointDataAccessor;
template<>
struct traits<JointAccessor>
{
enum {
NQ = -1, // Dynamic because unknown at compilation
NV = -1
};
typedef JointDataAccessor JointData;
typedef JointModelAccessor JointModel;
typedef ConstraintXd Constraint_t;
typedef SE3 Transformation_t;
typedef Motion Motion_t;
typedef Motion Bias_t;
typedef Eigen::Matrix<double,6,Eigen::Dynamic> F_t;
// [ABA]
typedef Eigen::Matrix<double,6,Eigen::Dynamic> U_t;
typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> D_t;
typedef Eigen::Matrix<double,6,Eigen::Dynamic> UD_t;
typedef Eigen::Matrix<double,Eigen::Dynamic,1> ConfigVector_t;
typedef Eigen::Matrix<double,Eigen::Dynamic,1> TangentVector_t;
};
template<> struct traits<JointDataAccessor> { typedef JointAccessor Joint; };
template<> struct traits<JointModelAccessor> { typedef JointAccessor Joint; };
struct JointDataAccessor : public JointDataBase<JointDataAccessor>
{
typedef JointAccessor Joint;
SE3_JOINT_TYPEDEF;
JointDataVariant & j_data_variant;
const Constraint_t S() const { return constraint_xd(j_data_variant); }
const Transformation_t M() const { return joint_transform(j_data_variant); } // featherstone book, page 78 (sXp)
const Motion_t v() const { return motion(j_data_variant); }
const Bias_t c() const { return bias(j_data_variant); }
// // [ABA CCRBA]
const U_t U() const { return u_inertia(j_data_variant); }
U_t U() { return u_inertia(j_data_variant); }
const D_t Dinv() const { return dinv_inertia(j_data_variant); }
const UD_t UDinv() const { return udinv_inertia(j_data_variant); }
// JointDataAccessor() {} // can become necessary if we want a vector of jointDataAccessor ?
JointDataAccessor( JointDataVariant & jdata ) : j_data_variant(jdata){}
};
struct JointModelAccessor : public JointModelBase<JointModelAccessor>
{
typedef JointAccessor Joint;
SE3_JOINT_TYPEDEF;
SE3_JOINT_USE_INDEXES;
using JointModelBase<JointModelAccessor>::id;
using JointModelBase<JointModelAccessor>::setIndexes;
const JointModelVariant & j_model_variant;
JointDataVariant createData()
{
return ::se3::createData(j_model_variant);
}
void calc (JointData & data,
const Eigen::VectorXd & qs) const
{
calc_zero_order(j_model_variant, data.j_data_variant, qs);
}
void calc (JointData & data,
const Eigen::VectorXd & qs,
const Eigen::VectorXd & vs ) const
{
calc_first_order(j_model_variant, data.j_data_variant, qs, vs);
}
void calc_aba(JointData & data, Inertia::Matrix6 & I, const bool update_I) const
{
::se3::calc_aba(j_model_variant, data.j_data_variant, I, update_I);
}
ConfigVector_t integrate_impl(const Eigen::VectorXd & qs,const Eigen::VectorXd & vs) const
{
return ::se3::integrate(j_model_variant, qs, vs);
}
ConfigVector_t interpolate_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1, const double u) const
{
return ::se3::interpolate(j_model_variant, q0, q1, u);
}
ConfigVector_t randomConfiguration_impl(const ConfigVector_t & lower_pos_limit, const ConfigVector_t & upper_pos_limit ) const throw (std::runtime_error)
{
return ::se3::randomConfiguration(j_model_variant, lower_pos_limit, upper_pos_limit);
}
TangentVector_t difference_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
return ::se3::difference(j_model_variant, q0, q1);
}
double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
return ::se3::distance(j_model_variant, q0, q1);
}
// JointModelAccessor() : j_model_variant() {} // can become necessary if we want a vector of jointModelAccessor ?
JointModelAccessor( const JointModelVariant & model_variant ) : j_model_variant(model_variant)
{}
int nq_impl() const { return ::se3::nq(j_model_variant); }
int nv_impl() const { return ::se3::nv(j_model_variant); }
int idx_q() const { return ::se3::idx_q(j_model_variant); }
int idx_v() const { return ::se3::idx_v(j_model_variant); }
JointIndex id() const { return ::se3::id(j_model_variant); }
void setIndexes(JointIndex ,int ,int ) { SE3_STATIC_ASSERT(false, THIS_METHOD_SHOULD_NOT_BE_CALLED_ON_DERIVED_CLASS); }
};
} // namespace se3
#endif // ifndef __se3_joint_accessor_hpp__
......@@ -260,8 +260,8 @@ namespace se3
/**
* @brief Integrate joint's configuration for a tangent vector during one unit time
*
* @param[in] q initatial configuration (size jmodel.nq)
* @param[in] v joint velocity (size jmodel.nv)
* @param[in] q initatial configuration (size full model.nq)
* @param[in] v joint velocity (size full model.nv)
*
* @return The configuration integrated
*/
......
//
// Copyright (c) 2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio 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.
//
// Pinocchio 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
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#ifndef __se3_joint_basic_visitors_hpp__
#define __se3_joint_basic_visitors_hpp__
#include <Eigen/StdVector>
#include <boost/variant.hpp>
#include "pinocchio/multibody/joint/joint-variant.hpp"
namespace se3
{
/**
* @brief Visit a JointModelVariant through CreateData visitor to create a JointDataVariant
*
* @param[in] jmodel The JointModelVariant we want to create a data for
*
* @return The created JointDataVariant
*/
inline JointDataVariant createData(const JointModelVariant & jmodel);
/**
* @brief Visit a JointDataVariant and the corresponding JointModelVariant through JointCalcZeroOrderVisitor
* to compute the joint data kinematics at order zero
*
* @param[in] jmodel The corresponding JointModelVariant to the JointDataVariant we want to update
* @param jdata The JointDataVariant we want to update
* @param[in] q The full model's (in which the joint belongs to) configuration vector
*/
inline void calc_zero_order(const JointModelVariant & jmodel, JointDataVariant & jdata, const Eigen::VectorXd & q);
/**
* @brief Visit a JointDataVariant and the corresponding JointModelVariant through JointCalcFirstOrderVisitor
* to compute the joint data kinematics at order one
*
* @param[in] jmodel The corresponding JointModelVariant to the JointDataVariant we want to update
* @param jdata The JointDataVariant we want to update
* @param[in] q The full model's (in which the joint belongs to) configuration vector
*/
inline void calc_first_order(const JointModelVariant & jmodel, JointDataVariant & jdata, const Eigen::VectorXd & q, const Eigen::VectorXd & v);
/**
* @brief Visit a JointDataVariant and the corresponding JointModelVariant through JointCalcAbaVisitor to
*
*
* @param[in] jmodel The corresponding JointModelVariant to the JointDataVariant we want to update
* @param jdata The JointDataVariant we want to update
* @param I Inertia matrix of the subtree following the jmodel in the kinematic chain as dense matrix
* @param[in] update_I If I should be updated or not
*/
inline void calc_aba(const JointModelVariant & jmodel, JointDataVariant & jdata, Inertia::Matrix6 & I, const bool update_I);
/**
* @brief Visit a JointModelVariant through JointIntegrateVisitor to integrate joint's configuration
* for a tangent vector during one unit time
*
* @param[in] jmodel The JointModelVariant
* @param[in] q Initatial configuration (size full model.nq)
* @param[in] v Joints velocity (size full model.nv)
*
* @return The configuration integrated
*/
inline Eigen::VectorXd integrate(const JointModelVariant & jmodel,
const Eigen::VectorXd & q,
const Eigen::VectorXd & v);
/**
* @brief Visit a JointModelVariant through JointInterpolateVisitor to compute
* the interpolation between two joint's configurations
*
* @param[in] jmodel The JointModelVariant
* @param[in] q0 Initial configuration to interpolate
* @param[in] q1 Final configuration to interpolate
* @param[in] u u in [0;1] position along the interpolation.
*
* @return The interpolated configuration (q0 if u = 0, q1 if u = 1)
*/
inline Eigen::VectorXd interpolate(const JointModelVariant & jmodel,
const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1,
const double u);
/**
* @brief Visit a JointModelVariant through JointRandomConfigurationVisitor to
* generate a configuration vector uniformly sampled among provided limits
*
* @param[in] jmodel The JointModelVariant
* @param[in] lower_pos_limit lower joint limit
* @param[in] upper_pos_limit upper joint limit
*
* @return The joint configuration
*/
inline Eigen::VectorXd randomConfiguration(const JointModelVariant & jmodel,
const Eigen::VectorXd & lower_pos_limit,
const Eigen::VectorXd & upper_pos_limit);
/**
* @brief Visit a JointModelVariant through JointDifferenceVisitor to compute
* the tangent vector that must be integrated during one unit time to go from q0 to q1
*
* @param[in] jmodel The JointModelVariant
* @param[in] q0 Initial configuration
* @param[in] q1 Wished configuration
*
* @return The corresponding velocity
*/
inline Eigen::VectorXd difference(const JointModelVariant & jmodel,
const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1);
/**
* @brief Visit a JointModelVariant through JointDifferenceVisitor to compute the distance
* between two configurations
*
* @param[in] jmodel The JointModelVariant
* @param[in] q0 Configuration 1
* @param[in] q1 Configuration 2
*
* @return The corresponding distance
*/
inline double distance(const JointModelVariant & jmodel,
const Eigen::VectorXd & q0,
const Eigen::VectorXd & q1);
/**
* @brief Visit a JointModelVariant through JointNvVisitor to get the dimension of
* the joint tangent space
*
* @param[in] jmodel The JointModelVariant
*
* @return The dimension of joint tangent space
*/
inline int nv(const JointModelVariant & jmodel);
/**
* @brief Visit a JointModelVariant through JointNqVisitor to get the dimension of
* the joint configuration space
*
* @param[in] jmodel The JointModelVariant
*
* @return The dimension of joint configuration space
*/
inline int nq(const JointModelVariant & jmodel);
/**
* @brief Visit a JointModelVariant through JointIdxQVisitor to get the index in the full model configuration
* space corresponding to the first degree of freedom of the Joint
*
* @param[in] jmodel The JointModelVariant
*
* @return The index in the full model configuration space corresponding to the first
* degree of freedom of jmodel
*/
inline int idx_q(const JointModelVariant & jmodel);
/**
* @brief Visit a JointModelVariant through JointIdxVVisitor to get the index in the full model tangent
* space corresponding to the first joint tangent space degree
*
* @param[in] jmodel The JointModelVariant
*
* @return The index in the full model tangent space corresponding to the first
* joint tangent space degree
*/
inline int idx_v(const JointModelVariant & jmodel);
/**
* @brief Visit a JointModelVariant through JointIdVisitor to get the index of the joint in the kinematic chain
*
* @param[in] jmodel The JointModelVariant
*
* @return The index of the joint in the kinematic chain
*/
inline JointIndex id(const JointModelVariant & jmodel);
/**
* @brief Visit a JointModelVariant through JointSetIndexesVisitor to set
* the indexes of the joint in the kinematic chain
*
* @param[in] jmodel The JointModelVariant
* @param[in] id The index of joint in the kinematic chain
* @param[in] q The index in the full model configuration space corresponding to the first degree of freedom
* @param[in] v The index in the full model tangent space corresponding to the first joint tangent space degree
*
* @return The index of the joint in the kinematic chain
*/
inline void setIndexes(JointModelVariant & jmodel, JointIndex id, int q,int v);
//
// Visitors on JointDatas
//
/**
* @brief Visit a JointDataVariant through JointConstraintVisitor to get the joint constraint
* as a dense constraint
*
* @param[in] jdata The jdata
*
* @return The constraint dense corresponding to the joint derived constraint
*/
inline ConstraintXd constraint_xd(const JointDataVariant & jdata);
/**
* @brief Visit a JointDataVariant through JointTransformVisitor to get the joint internal transform (transform
* between the entry frame and the exit frame of the joint)
*
* @param[in] jdata The jdata
*
* @return The joint transform corresponding to the joint derived transform (sXp)
*/
inline SE3 joint_transform(const JointDataVariant & jdata);
/**
* @brief Visit a JointDataVariant through JointMotionVisitor to get the joint internal motion
* as a dense motion
*
* @param[in] jdata The jdata
*
* @return The motion dense corresponding to the joint derived motion
*/
inline Motion motion(const JointDataVariant & jdata);
/**
* @brief Visit a JointDataVariant through JointBiasVisitor to get the joint bias
* as a dense motion
*
* @param[in] jdata The jdata
*
* @return The motion dense corresponding to the joint derived bias
*/
inline Motion bias(const JointDataVariant & jdata);
/**
* @brief Visit a JointDataVariant through JointUInertiaVisitor to get the U matrix of the inertia matrix
* decomposition
*
* @param[in] jdata The jdata
*
* @return The U matrix of the inertia matrix decomposition
*/
inline Eigen::Matrix<double,6,Eigen::Dynamic> u_inertia(const JointDataVariant & jdata);
/**
* @brief Visit a JointDataVariant through JointDInvInertiaVisitor to get the D^{-1} matrix of the inertia matrix
* decomposition
*
* @param[in] jdata The jdata
*
* @return The D^{-1} matrix of the inertia matrix decomposition
*/
inline Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> dinv_inertia(const JointDataVariant & jdata);
/**
* @brief Visit a JointDataVariant through JointUDInvInertiaVisitor to get U*D^{-1} matrix of the inertia matrix
* decomposition
*
* @param[in] jdata The jdata
*
* @return The U*D^{-1} matrix of the inertia matrix decomposition
*/
inline Eigen::Matrix<double,6,Eigen::Dynamic> udinv_inertia(const JointDataVariant & jdata);
} // namespace se3
/* --- Details -------------------------------------------------------------------- */
#include "pinocchio/multibody/joint/joint-basic-visitors.hxx"
#endif // ifndef __se3_joint_basic_visitors_hpp__
//
// Copyright (c) 2016 CNRS
//
// This file is part of Pinocchio
// Pinocchio 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.
//
// Pinocchio 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
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.
#ifndef __se3_joint_basic_visitors_hxx__
#define __se3_joint_basic_visitors_hxx__
// remove the includes ?
#include <Eigen/StdVector>
#include <boost/variant.hpp>
#include "pinocchio/assert.hpp"
#include "pinocchio/multibody/joint/joint-variant.hpp"
namespace se3
{
/// @cond DEV
/**
* @brief CreateJointData visitor
*/
class CreateJointData: public boost::static_visitor<JointDataVariant>
{
public:
template<typename D>
JointDataVariant operator()(const JointModelBase<D> & jmodel) const
{ return JointDataVariant(jmodel.createData()); }
static JointDataVariant run( const JointModelVariant & jmodel)
{ return boost::apply_visitor( CreateJointData(), jmodel ); }
};
inline JointDataVariant createData(const JointModelVariant & jmodel)
{
return CreateJointData::run(jmodel);
}
/**
* @brief JointCalcZeroOrderVisitor
*/
class JointCalcZeroOrderVisitor: public boost::static_visitor<>
{
public:
const Eigen::VectorXd & q;
JointCalcZeroOrderVisitor(const Eigen::VectorXd & q) : q(q) {}
template<typename D1, typename D2>
void operator()(const JointModelBase<D1> & , JointDataBase<D2> & ) const
{
SE3_STATIC_ASSERT(false, MIXING_JOINT_MODEL_AND_DATA_OF_DIFFERENT_TYPE);
}
template<typename D>
void operator()(const JointModelBase<D> & jmodel, JointDataBase<D> & jdata) const
{
jmodel.calc(jdata,q);
}
static void run( const JointModelVariant & jmodel, JointDataVariant & jdata, const Eigen::VectorXd & q)
{ boost::apply_visitor( JointCalcZeroOrderVisitor(q), jmodel, jdata ); }
};
inline void calc_zero_order(const JointModelVariant & jmodel, JointDataVariant & jdata, const Eigen::VectorXd & q)
{
JointCalcZeroOrderVisitor::run( jmodel, jdata, q );
}
/**
* @brief JointCalcFirstOrderVisitor
*/
class JointCalcFirstOrderVisitor: public boost::static_visitor<>
{
public:
const Eigen::VectorXd & q;
const Eigen::VectorXd & v;
JointCalcFirstOrderVisitor(const Eigen::VectorXd & q, const Eigen::VectorXd & v) : q(q), v(v) {}
template<typename D1, typename D2>
void operator()(const JointModelBase<D1> & , JointDataBase<D2> & ) const
{
SE3_STATIC_ASSERT(false, MIXING_JOINT_MODEL_AND_DATA_OF_DIFFERENT_TYPE);
}
template<typename D>
void operator()(const JointModelBase<D> & jmodel, JointDataBase<D> & jdata) const
{
jmodel.calc(jdata,q,v);
}
static void run( const JointModelVariant & jmodel, JointDataVariant & jdata, const Eigen::VectorXd & q, const Eigen::VectorXd & v)
{ boost::apply_visitor( JointCalcFirstOrderVisitor(q, v), jmodel, jdata ); }
};