joint-revolute-unbounded.hpp

//
// Copyright (c) 2016-2020 CNRS INRIA
//

#ifndef __pinocchio_multibody_joint_revolute_unbounded_hpp__
#define __pinocchio_multibody_joint_revolute_unbounded_hpp__

#include "pinocchio/math/fwd.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"

namespace pinocchio
{

 template<typename Scalar, int Options, int axis>
 struct JointRevoluteUnboundedTpl;

 template<typename _Scalar, int _Options, int axis>
 struct traits<JointRevoluteUnboundedTpl<_Scalar, _Options, axis>>
 {
 enum
 {
 NQ = 2,
 NV = 1,
 NVExtended = 1
 };
 typedef _Scalar Scalar;
 enum
 {
 Options = _Options
 };
 typedef JointDataRevoluteUnboundedTpl<Scalar, Options, axis> JointDataDerived;
 typedef JointModelRevoluteUnboundedTpl<Scalar, Options, axis> JointModelDerived;
 typedef JointMotionSubspaceRevoluteTpl<Scalar, Options, axis> Constraint_t;
 typedef TransformRevoluteTpl<Scalar, Options, axis> Transformation_t;
 typedef MotionRevoluteTpl<Scalar, Options, axis> Motion_t;
 typedef MotionZeroTpl<Scalar, Options> Bias_t;

 // [ABA]
 typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
 typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
 typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;

 typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
 typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;

 typedef boost::mpl::true_ is_mimicable_t;

 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
 };

 template<typename _Scalar, int _Options, int axis>
 struct traits<JointDataRevoluteUnboundedTpl<_Scalar, _Options, axis>>
 {
 typedef JointRevoluteUnboundedTpl<_Scalar, _Options, axis> JointDerived;
 typedef _Scalar Scalar;
 };

 template<typename _Scalar, int _Options, int axis>
 struct traits<JointModelRevoluteUnboundedTpl<_Scalar, _Options, axis>>
 {
 typedef JointRevoluteUnboundedTpl<_Scalar, _Options, axis> JointDerived;
 typedef _Scalar Scalar;
 };

 template<typename _Scalar, int _Options, int axis>
 struct JointDataRevoluteUnboundedTpl
 : public JointDataBase<JointDataRevoluteUnboundedTpl<_Scalar, _Options, axis>>
 {
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 typedef JointRevoluteUnboundedTpl<_Scalar, _Options, axis> JointDerived;
 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR

 ConfigVector_t joint_q;
 TangentVector_t joint_v;

 Constraint_t S;
 Transformation_t M;
 Motion_t v;
 Bias_t c;

 // [ABA] specific data
 U_t U;
 D_t Dinv;
 UD_t UDinv;
 D_t StU;

 JointDataRevoluteUnboundedTpl()
 : joint_q(Scalar(1), Scalar(0))
 , joint_v(TangentVector_t::Zero())
 , M((Scalar)0, (Scalar)1)
 , v((Scalar)0)
 , U(U_t::Zero())
 , Dinv(D_t::Zero())
 , UDinv(UD_t::Zero())
 , StU(D_t::Zero())
 {
 }

 static std::string classname()
 {
 return std::string("JointDataRUB") + axisLabel<axis>();
 }
 std::string shortname() const
 {
 return classname();
 }

 }; // struct JointDataRevoluteUnbounded

 template<typename NewScalar, typename Scalar, int Options, int axis>
 struct CastType<NewScalar, JointModelRevoluteUnboundedTpl<Scalar, Options, axis>>
 {
 typedef JointModelRevoluteUnboundedTpl<NewScalar, Options, axis> type;
 };

 template<typename _Scalar, int _Options, int axis>
 struct JointModelRevoluteUnboundedTpl
 : public JointModelBase<JointModelRevoluteUnboundedTpl<_Scalar, _Options, axis>>
 {
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 typedef JointRevoluteUnboundedTpl<_Scalar, _Options, axis> JointDerived;
 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
 typedef JointRevoluteTpl<Scalar, _Options, axis> JointDerivedBase;

 typedef JointModelBase<JointModelRevoluteUnboundedTpl> Base;
 using Base::id;
 using Base::idx_q;
 using Base::idx_v;
 using Base::idx_vExtended;
 using Base::setIndexes;

 typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;

 JointDataDerived createData() const
 {
 return JointDataDerived();
 }

 const std::vector<bool> hasConfigurationLimit() const
 {
 return {false, false};
 }

 const std::vector<bool> hasConfigurationLimitInTangent() const
 {
 return {false};
 }

 template<typename ConfigVector>
 void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
 {
 data.joint_q = qs.template segment<NQ>(idx_q());

 const Scalar & ca = data.joint_q[0];
 const Scalar & sa = data.joint_q[1];

 data.M.setValues(sa, ca);
 }

 template<typename TangentVector>
 void
 calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
 const
 {
 data.joint_v[0] = vs[idx_v()];
 data.v.angularRate() = data.joint_v[0];
 }

 template<typename ConfigVector, typename TangentVector>
 void calc(
 JointDataDerived & data,
 const typename Eigen::MatrixBase<ConfigVector> & qs,
 const typename Eigen::MatrixBase<TangentVector> & vs) const
 {
 calc(data, qs.derived());
 data.joint_v[0] = vs[idx_v()];
 data.v.angularRate() = data.joint_v[0];
 }

 template<typename VectorLike, typename Matrix6Like>
 void calc_aba(
 JointDataDerived & data,
 const Eigen::MatrixBase<VectorLike> & armature,
 const Eigen::MatrixBase<Matrix6Like> & I,
 const bool update_I) const
 {
 data.U = I.col(Inertia::ANGULAR + axis);
 data.Dinv[0] =
 (Scalar)(1) / (I(Inertia::ANGULAR + axis, Inertia::ANGULAR + axis) + armature[0]);
 data.UDinv.noalias() = data.U * data.Dinv[0];

 if (update_I)
 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
 }

 static std::string classname()
 {
 return std::string("JointModelRUB") + axisLabel<axis>();
 }
 std::string shortname() const
 {
 return classname();
 }

 Vector3 getMotionAxis() const
 {
 switch (axis)
 {
 case 0:
 return Vector3::UnitX();
 case 1:
 return Vector3::UnitY();
 case 2:
 return Vector3::UnitZ();
 default:
 assert(false && "must never happen");
 break;
 }
 }

 template<typename NewScalar>
 JointModelRevoluteUnboundedTpl<NewScalar, Options, axis> cast() const
 {
 typedef JointModelRevoluteUnboundedTpl<NewScalar, Options, axis> ReturnType;
 ReturnType res;
 res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended());
 return res;
 }

 }; // struct JointModelRevoluteUnboundedTpl

 typedef JointRevoluteUnboundedTpl<context::Scalar, context::Options, 0> JointRUBX;
 typedef JointDataRevoluteUnboundedTpl<context::Scalar, context::Options, 0> JointDataRUBX;
 typedef JointModelRevoluteUnboundedTpl<context::Scalar, context::Options, 0> JointModelRUBX;

 typedef JointRevoluteUnboundedTpl<context::Scalar, context::Options, 1> JointRUBY;
 typedef JointDataRevoluteUnboundedTpl<context::Scalar, context::Options, 1> JointDataRUBY;
 typedef JointModelRevoluteUnboundedTpl<context::Scalar, context::Options, 1> JointModelRUBY;

 typedef JointRevoluteUnboundedTpl<context::Scalar, context::Options, 2> JointRUBZ;
 typedef JointDataRevoluteUnboundedTpl<context::Scalar, context::Options, 2> JointDataRUBZ;
 typedef JointModelRevoluteUnboundedTpl<context::Scalar, context::Options, 2> JointModelRUBZ;

 template<typename Scalar, int Options, int axis>
 struct ConfigVectorAffineTransform<JointRevoluteUnboundedTpl<Scalar, Options, axis>>
 {
 typedef UnboundedRevoluteAffineTransform Type;
 };
} // namespace pinocchio

#include <boost/type_traits.hpp>

namespace boost
{
 template<typename Scalar, int Options, int axis>
 struct has_nothrow_constructor<::pinocchio::JointModelRevoluteUnboundedTpl<Scalar, Options, axis>>
 : public integral_constant<bool, true>
 {
 };

 template<typename Scalar, int Options, int axis>
 struct has_nothrow_copy<::pinocchio::JointModelRevoluteUnboundedTpl<Scalar, Options, axis>>
 : public integral_constant<bool, true>
 {
 };

 template<typename Scalar, int Options, int axis>
 struct has_nothrow_constructor<::pinocchio::JointDataRevoluteUnboundedTpl<Scalar, Options, axis>>
 : public integral_constant<bool, true>
 {
 };

 template<typename Scalar, int Options, int axis>
 struct has_nothrow_copy<::pinocchio::JointDataRevoluteUnboundedTpl<Scalar, Options, axis>>
 : public integral_constant<bool, true>
 {
 };
} // namespace boost

#endif // ifndef __pinocchio_multibody_joint_revolute_unbounded_hpp__