Commit d7199bb8 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #242 from fvalenza/topic/JointRevoluteUnbounded

Topic/joint revolute unbounded
parents f16029b5 0e6affcc
......@@ -139,6 +139,7 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
multibody/joint/joint-dense.hpp
multibody/joint/joint-revolute.hpp
multibody/joint/joint-revolute-unaligned.hpp
multibody/joint/joint-revolute-unbounded.hpp
multibody/joint/joint-spherical.hpp
multibody/joint/joint-spherical-ZYX.hpp
multibody/joint/joint-prismatic.hpp
......
......@@ -58,7 +58,7 @@ IF(BUILD_PYTHON_INTERFACE)
ENDIF(BUILD_PYTHON_INTERFACE)
IF(LUA5_1_FOUND)
LIST(APPEND ${PROJECT_NAME}_PARSER_SOURCES
LIST(APPEND ${PROJECT_NAME}_PARSERS_SOURCES
parsers/lua/lua_tables.cpp
parsers/lua.cpp
)
......
......@@ -428,7 +428,7 @@ namespace se3
double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
return difference_impl(q0,q1).norm();
return fabs(difference_impl(q0,q1)[0]);
}
ConfigVector_t neutralConfiguration_impl() const
......
......@@ -483,7 +483,7 @@ namespace se3
double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
return difference_impl(q0,q1).norm();
return fabs(difference_impl(q0,q1)[0]);
}
ConfigVector_t neutralConfiguration_impl() const
......
......@@ -432,7 +432,7 @@ namespace se3
double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
return difference_impl(q0,q1).norm();
return fabs(difference_impl(q0,q1)[0]);
}
ConfigVector_t neutralConfiguration_impl() const
......
//
// 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_revolute_unbounded_hpp__
#define __se3_joint_revolute_unbounded_hpp__
#include "pinocchio/math/sincos.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/joint/joint-revolute.hpp"
#include "pinocchio/multibody/joint/joint-dense.hpp"
#include <stdexcept>
namespace se3
{
template<int axis> struct JointRevoluteUnbounded;
template<int axis> struct JointDataRevoluteUnbounded;
template<int axis> struct JointModelRevoluteUnbounded;
template<int axis>
struct traits< JointRevoluteUnbounded<axis> >
{
enum {
NQ = 2,
NV = 1
};
typedef JointDataRevoluteUnbounded<axis> JointDataDerived;
typedef JointModelRevoluteUnbounded<axis> JointModelDerived;
typedef ConstraintRevolute<axis> Constraint_t;
typedef SE3 Transformation_t;
typedef MotionRevolute<axis> Motion_t;
typedef BiasZero Bias_t;
typedef Eigen::Matrix<double,6,NV> F_t;
// [ABA]
typedef Eigen::Matrix<double,6,NV> U_t;
typedef Eigen::Matrix<double,NV,NV> D_t;
typedef Eigen::Matrix<double,6,NV> UD_t;
typedef Eigen::Matrix<double,NQ,1> ConfigVector_t;
typedef Eigen::Matrix<double,NV,1> TangentVector_t;
};
template<int axis> struct traits< JointDataRevoluteUnbounded<axis> > { typedef JointRevoluteUnbounded<axis> JointDerived; };
template<int axis> struct traits< JointModelRevoluteUnbounded<axis> > { typedef JointRevoluteUnbounded<axis> JointDerived; };
template<int axis>
struct JointDataRevoluteUnbounded : public JointDataBase< JointDataRevoluteUnbounded<axis> >
{
typedef JointRevoluteUnbounded<axis> JointDerived;
SE3_JOINT_TYPEDEF_TEMPLATE;
Constraint_t S;
Transformation_t M;
Motion_t v;
Bias_t c;
F_t F;
// [ABA] specific data
U_t U;
D_t Dinv;
UD_t UDinv;
JointDataRevoluteUnbounded() : M(1), U(), Dinv(), UDinv()
{}
JointDataDense<NQ, NV> toDense_impl() const
{
return JointDataDense<NQ, NV>(S, M, v, c, F, U, Dinv, UDinv);
}
}; // struct JointDataRevoluteUnbounded
template<int axis>
struct JointModelRevoluteUnbounded : public JointModelBase< JointModelRevoluteUnbounded<axis> >
{
typedef JointRevoluteUnbounded<axis> JointDerived;
SE3_JOINT_TYPEDEF_TEMPLATE;
using JointModelBase<JointModelRevoluteUnbounded>::id;
using JointModelBase<JointModelRevoluteUnbounded>::idx_q;
using JointModelBase<JointModelRevoluteUnbounded>::idx_v;
using JointModelBase<JointModelRevoluteUnbounded>::setIndexes;
typedef Motion::Vector3 Vector3;
typedef double Scalar;
JointDataDerived createData() const { return JointDataDerived(); }
void calc( JointDataDerived& data,
const Eigen::VectorXd & qs ) const
{
typename Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q = qs.segment<NQ> (idx_q ());
const double & ca = q(0);
const double & sa = q(1);
data.M.rotation(JointRevolute<axis>::cartesianRotation(ca,sa));
}
void calc( JointDataDerived& data,
const Eigen::VectorXd & qs,
const Eigen::VectorXd & vs ) const
{
typename Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q = qs.segment<NQ> (idx_q ());
typename Eigen::VectorXd::ConstFixedSegmentReturnType<NV>::Type & q_dot = vs.segment<NV> (idx_v ());
const double & ca = q(0);
const double & sa = q(1);
const double & v = q_dot(0);
data.M.rotation(JointRevolute<axis>::cartesianRotation(ca,sa));
data.v.w = v;
}
void calc_aba(JointDataDerived & data, Inertia::Matrix6 & I, const bool update_I) const
{
data.U = I.col(Inertia::ANGULAR + axis);
data.Dinv[0] = 1./I(Inertia::ANGULAR + axis,Inertia::ANGULAR + axis);
data.UDinv = data.U * data.Dinv[0];
if (update_I)
I -= data.UDinv * data.U.transpose();
}
ConfigVector_t integrate_impl(const Eigen::VectorXd & qs,const Eigen::VectorXd & vs) const
{
typename Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q = qs.segment<NQ> (idx_q ());
typename Eigen::VectorXd::ConstFixedSegmentReturnType<NV>::Type & q_dot = vs.segment<NV> (idx_v ());
const double & ca = q(0);
const double & sa = q(1);
const double & omega = q_dot(0);
double cosOmega,sinOmega; SINCOS (omega, &sinOmega, &cosOmega);
double norm2p = q.squaredNorm();
ConfigVector_t result;
result << (1.5-.5*norm2p) * (cosOmega * ca - sinOmega * sa),
(1.5-.5*norm2p) * (sinOmega * ca + cosOmega * sa);
return result;
}
ConfigVector_t interpolate_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1, const double u) const
{
typename Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & qi = q0.segment<NQ> (idx_q ());
typename Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & qf = q1.segment<NQ> (idx_q ());
const double & c0 = qi(0), s0 = qi(1);
const double & c1 = qf(0), s1 = qf(1);
assert ( (qi.norm() - 1) < 1e-8 && "initial configuration not normalized");
assert ( (qf.norm() - 1) < 1e-8 && "final configuration not normalized");
double cosTheta = c0*c1 + s0*s1;
double sinTheta = c0*s1 - s0*c1;
double theta = atan2(sinTheta, cosTheta);
assert (fabs (sin (theta) - sinTheta) < 1e-8);
ConfigVector_t result;
if (fabs (theta) > 1e-6 && fabs (theta) < PI - 1e-6)
{
result = (sin ((1-u)*theta)/sinTheta) * qi
+ (sin (u*theta)/sinTheta) * qf;
}
else if (fabs (theta) < 1e-6) // theta = 0
{
result = (1-u) * qi + u * qf;
}
else // theta = +-PI
{
double theta0 = atan2 (s0, c0);
result << cos (theta0 + u * theta),
sin (theta0 + u * theta);
}
return result;
}
ConfigVector_t random_impl() const
{
ConfigVector_t result(ConfigVector_t::Random());
result.normalize();
return result;
}
ConfigVector_t randomConfiguration_impl(const ConfigVector_t & , const ConfigVector_t & ) const
{
ConfigVector_t result;
double angle = -PI + 2*PI * rand()/RAND_MAX;
double ca,sa; SINCOS (angle, &sa, &ca);
result << ca, sa;
return result;
}
TangentVector_t difference_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
typename Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & qi = q0.segment<NQ> (idx_q ());
typename Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & qf = q1.segment<NQ> (idx_q ());
const double & c0 = qi(0), s0 = qi(1);
const double & c1 = qf(0), s1 = qf(1);
TangentVector_t result;
result << atan2 (s0*c1 - s1*c0, c0*c1 + s0*s1);
return result;
}
double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
return fabs(difference_impl(q0,q1)[0]);
}
ConfigVector_t neutralConfiguration_impl() const
{
ConfigVector_t q;
q << 1,0;
return q;
}
void normalize_impl(Eigen::VectorXd& q) const
{
q.segment<NQ>(idx_q()).normalize();
}
JointModelDense<NQ, NV> toDense_impl() const
{
return JointModelDense<NQ, NV>( id(),
idx_q(),
idx_v()
);
}
static const std::string shortname();
template <class D>
bool operator == (const JointModelBase<D> &) const
{
return false;
}
bool operator == (const JointModelBase<JointModelRevoluteUnbounded <axis> > & jmodel) const
{
return jmodel.id() == id()
&& jmodel.idx_q() == idx_q()
&& jmodel.idx_v() == idx_v();
}
}; // struct JointModelRevoluteUnbounded
typedef JointRevoluteUnbounded<0> JointRUBX;
typedef JointDataRevoluteUnbounded<0> JointDataRUBX;
typedef JointModelRevoluteUnbounded<0> JointModelRUBX;
template<> inline
const std::string JointModelRevoluteUnbounded<0>::shortname()
{
return std::string("JointModelRUBX") ;
}
typedef JointRevoluteUnbounded<1> JointRUBY;
typedef JointDataRevoluteUnbounded<1> JointDataRUBY;
typedef JointModelRevoluteUnbounded<1> JointModelRUBY;
template<> inline
const std::string JointModelRevoluteUnbounded<1>::shortname()
{
return std::string("JointModelRUBY") ;
}
typedef JointRevoluteUnbounded<2> JointRUBZ;
typedef JointDataRevoluteUnbounded<2> JointDataRUBZ;
typedef JointModelRevoluteUnbounded<2> JointModelRUBZ;
template<> inline
const std::string JointModelRevoluteUnbounded<2>::shortname()
{
return std::string("JointModelRUBZ") ;
}
} //namespace se3
#endif // ifndef __se3_joint_revolute_unbounded_hpp__
......@@ -205,7 +205,7 @@ namespace se3
template<int axis>
struct JointRevolute {
static Eigen::Matrix3d cartesianRotation(const double & angle);
static Eigen::Matrix3d cartesianRotation(const double & ca, const double & sa);
};
inline Motion operator^( const Motion& m1, const MotionRevolute<0>& m2)
......@@ -254,10 +254,9 @@ namespace se3
}
template<> inline
Eigen::Matrix3d JointRevolute<0>::cartesianRotation(const double & angle)
Eigen::Matrix3d JointRevolute<0>::cartesianRotation(const double & ca, const double & sa)
{
Eigen::Matrix3d R3;
double ca,sa; SINCOS (angle,&sa,&ca);
R3 <<
1,0,0,
0,ca,-sa,
......@@ -266,10 +265,9 @@ namespace se3
}
template<> inline
Eigen::Matrix3d JointRevolute<1>::cartesianRotation(const double & angle)
Eigen::Matrix3d JointRevolute<1>::cartesianRotation(const double & ca, const double & sa)
{
Eigen::Matrix3d R3;
double ca,sa; SINCOS (angle,&sa,&ca);
Eigen::Matrix3d R3;
R3 <<
ca, 0, sa,
0, 1, 0,
......@@ -278,10 +276,9 @@ namespace se3
}
template<> inline
Eigen::Matrix3d JointRevolute<2>::cartesianRotation(const double & angle)
Eigen::Matrix3d JointRevolute<2>::cartesianRotation(const double & ca, const double & sa)
{
Eigen::Matrix3d R3;
double ca,sa; SINCOS (angle,&sa,&ca);
Eigen::Matrix3d R3;
R3 <<
ca,-sa,0,
sa,ca,0,
......@@ -437,7 +434,8 @@ namespace se3
const Eigen::VectorXd & qs ) const
{
const double & q = qs[idx_q()];
data.M.rotation(JointRevolute<axis>::cartesianRotation(q));
double ca,sa; SINCOS (q,&sa,&ca);
data.M.rotation(JointRevolute<axis>::cartesianRotation(ca, sa));
}
void calc( JointDataDerived& data,
......@@ -447,7 +445,9 @@ namespace se3
const double & q = qs[idx_q()];
const double & v = vs[idx_v()];
data.M.rotation(JointRevolute<axis>::cartesianRotation(q));
double ca,sa; SINCOS (q,&sa,&ca);
data.M.rotation(JointRevolute<axis>::cartesianRotation(ca, sa));
data.v.w = v;
}
......@@ -517,7 +517,7 @@ namespace se3
double distance_impl(const Eigen::VectorXd & q0,const Eigen::VectorXd & q1) const
{
return difference_impl(q0,q1).norm();
return fabs(difference_impl(q0,q1)[0]);
}
ConfigVector_t neutralConfiguration_impl() const
......
......@@ -26,6 +26,7 @@
#include "pinocchio/multibody/joint/joint-prismatic-unaligned.hpp"
#include "pinocchio/multibody/joint/joint-revolute-unaligned.hpp"
#include "pinocchio/multibody/joint/joint-revolute.hpp"
#include "pinocchio/multibody/joint/joint-revolute-unbounded.hpp"
#include "pinocchio/multibody/joint/joint-spherical-ZYX.hpp"
#include "pinocchio/multibody/joint/joint-spherical.hpp"
#include "pinocchio/multibody/joint/joint-translation.hpp"
......@@ -37,8 +38,8 @@ namespace se3
{
enum { MAX_JOINT_NV = 6 };
typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelSpherical, JointModelSphericalZYX, JointModelPX, JointModelPY, JointModelPZ, JointModelPrismaticUnaligned, JointModelFreeFlyer, JointModelPlanar, JointModelTranslation, JointModelDense<-1,-1> > JointModelVariant;
typedef boost::variant< JointDataRX, JointDataRY, JointDataRZ, JointDataRevoluteUnaligned, JointDataSpherical, JointDataSphericalZYX, JointDataPX, JointDataPY, JointDataPZ, JointDataPrismaticUnaligned, JointDataFreeFlyer, JointDataPlanar, JointDataTranslation, JointDataDense<-1,-1> > JointDataVariant;
typedef boost::variant< JointModelRX, JointModelRY, JointModelRZ, JointModelRevoluteUnaligned, JointModelSpherical, JointModelSphericalZYX, JointModelPX, JointModelPY, JointModelPZ, JointModelPrismaticUnaligned, JointModelFreeFlyer, JointModelPlanar, JointModelTranslation, JointModelDense<-1,-1>, JointModelRUBX, JointModelRUBY, JointModelRUBZ > JointModelVariant;
typedef boost::variant< JointDataRX, JointDataRY, JointDataRZ, JointDataRevoluteUnaligned, JointDataSpherical, JointDataSphericalZYX, JointDataPX, JointDataPY, JointDataPZ, JointDataPrismaticUnaligned, JointDataFreeFlyer, JointDataPlanar, JointDataTranslation, JointDataDense<-1,-1>, JointDataRUBX, JointDataRUBY, JointDataRUBZ > JointDataVariant;
typedef std::vector<JointModelVariant> JointModelVariantVector;
typedef std::vector<JointDataVariant> JointDataVariantVector;
......
......@@ -44,6 +44,15 @@ void test_joint_methods (T & jmodel, typename T::JointDataDerived & jdata)
se3::Inertia::Matrix6 Ia(se3::Inertia::Random().matrix());
bool update_I = false;
if(T::shortname() == "JointModelRUBX" ||
T::shortname() == "JointModelRUBY" ||
T::shortname() == "JointModelRUBZ")
{
// normalize cos/sin
q1.normalize();
q2.normalize();
}
jmodel.calc(jdata, q1, q1_dot);
jmodel.calc_aba(jdata, Ia, update_I);
......
......@@ -25,6 +25,7 @@
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/multibody/joint/joint-revolute.hpp"
#include "pinocchio/multibody/joint/joint-revolute-unaligned.hpp"
#include "pinocchio/multibody/joint/joint-revolute-unbounded.hpp"
#include "pinocchio/multibody/joint/joint-spherical.hpp"
#include "pinocchio/multibody/joint/joint-spherical-ZYX.hpp"
#include "pinocchio/multibody/joint/joint-prismatic.hpp"
......@@ -835,3 +836,88 @@ BOOST_AUTO_TEST_CASE (vsFreeFlyer)
}
BOOST_AUTO_TEST_SUITE_END ()
BOOST_AUTO_TEST_SUITE (JointRevoluteUnbounded)
BOOST_AUTO_TEST_CASE (vsRX)
{
using namespace se3;
typedef Eigen::Matrix <double, 3, 1> Vector3;
typedef Eigen::Matrix <double, 6, 1> Vector6;
typedef Eigen::Matrix <double, 3, 3> Matrix3;
Model modelRX, modelRevoluteUnbounded;
Inertia inertia (1., Vector3 (0.5, 0., 0.0), Matrix3::Identity ());
SE3 pos(1); pos.translation() = SE3::Linear_t(1.,0.,0.);
JointModelRUBX joint_model_RUX;
modelRX.addJointAndBody (0, JointModelRX (), pos, inertia, "px");
modelRevoluteUnbounded.addJointAndBody(0, joint_model_RUX ,pos, inertia, "revolute unbounded x");
Data dataRX(modelRX);
Data dataRevoluteUnbounded(modelRevoluteUnbounded);
Eigen::VectorXd q_rx = Eigen::VectorXd::Ones (modelRX.nq);
Eigen::VectorXd q_rubx = Eigen::VectorXd::Ones (modelRevoluteUnbounded.nq);
double ca, sa; double alpha = q_rx(0); SINCOS (alpha, &sa, &ca);
q_rubx(0) = ca;
q_rubx(1) = sa;
Eigen::VectorXd v_rx = Eigen::VectorXd::Ones (modelRX.nv);
Eigen::VectorXd v_rubx = v_rx;
Eigen::VectorXd tauRX = Eigen::VectorXd::Ones (modelRX.nv); Eigen::VectorXd tauRevoluteUnbounded = Eigen::VectorXd::Ones (modelRevoluteUnbounded.nv);
Eigen::VectorXd aRX = Eigen::VectorXd::Ones (modelRX.nv); Eigen::VectorXd aRevoluteUnbounded = aRX;
forwardKinematics(modelRX, dataRX, q_rx, v_rx);
forwardKinematics(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx, v_rubx);
computeAllTerms(modelRX, dataRX, q_rx, v_rx);
computeAllTerms(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx, v_rubx);
BOOST_CHECK(dataRevoluteUnbounded.oMi[1].isApprox(dataRX.oMi[1]));
BOOST_CHECK(dataRevoluteUnbounded.liMi[1].isApprox(dataRX.liMi[1]));
BOOST_CHECK(dataRevoluteUnbounded.Ycrb[1].matrix().isApprox(dataRX.Ycrb[1].matrix()));
BOOST_CHECK(dataRevoluteUnbounded.f[1].toVector().isApprox(dataRX.f[1].toVector()));
BOOST_CHECK(dataRevoluteUnbounded.nle.isApprox(dataRX.nle));
BOOST_CHECK(dataRevoluteUnbounded.com[0].isApprox(dataRX.com[0]));
// InverseDynamics == rnea
tauRX = rnea(modelRX, dataRX, q_rx, v_rx, aRX);
tauRevoluteUnbounded = rnea(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx, v_rubx, aRevoluteUnbounded);
BOOST_CHECK(tauRX.isApprox(tauRevoluteUnbounded));
// ForwardDynamics == aba
Eigen::VectorXd aAbaRX= aba(modelRX,dataRX, q_rx, v_rx, tauRX);
Eigen::VectorXd aAbaRevoluteUnbounded = aba(modelRevoluteUnbounded,dataRevoluteUnbounded, q_rubx, v_rubx, tauRevoluteUnbounded);
BOOST_CHECK(aAbaRX.isApprox(aAbaRevoluteUnbounded));
// crba
crba(modelRX, dataRX,q_rx);
crba(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx);
BOOST_CHECK(dataRX.M.isApprox(dataRevoluteUnbounded.M));
// Jacobian
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPX;jacobianPX.resize(6,1); jacobianPX.setZero();
Eigen::Matrix<double, 6, Eigen::Dynamic> jacobianPrismaticUnaligned;jacobianPrismaticUnaligned.resize(6,1);jacobianPrismaticUnaligned.setZero();
computeJacobians(modelRX, dataRX, q_rx);
computeJacobians(modelRevoluteUnbounded, dataRevoluteUnbounded, q_rubx);
getJacobian<true>(modelRX, dataRX, 1, jacobianPX);
getJacobian<true>(modelRevoluteUnbounded, dataRevoluteUnbounded, 1, jacobianPrismaticUnaligned);
BOOST_CHECK(jacobianPX.isApprox(jacobianPrismaticUnaligned));
}
BOOST_AUTO_TEST_SUITE_END ()
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