<commit_before>#ifndef __se3_joint_revolute_hpp__
#define __se3_joint_revolute_hpp__

#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/math/sincos.hpp"

namespace se3
{

  template<int axis> struct JointDataRevolute;
  template<int axis> struct JointModelRevolute;
  
  namespace revolute
  {
    template<int axis>
    struct CartesianVector3
    {
      double w; 
      CartesianVector3(const double & w) : w(w) {}
      CartesianVector3() : w(1) {}
      operator Eigen::Vector3d (); // { return Eigen::Vector3d(w,0,0); }
    };
    template<>CartesianVector3<0>::operator Eigen::Vector3d () { return Eigen::Vector3d(w,0,0); }
    template<>CartesianVector3<1>::operator Eigen::Vector3d () { return Eigen::Vector3d(0,w,0); }
    template<>CartesianVector3<2>::operator Eigen::Vector3d () { return Eigen::Vector3d(0,0,w); }
    Eigen::Vector3d operator+ (const Eigen::Vector3d & w1,const CartesianVector3<0> & wx)
    { return Eigen::Vector3d(w1[0]+wx.w,w1[1],w1[2]); }
    Eigen::Vector3d operator+ (const Eigen::Vector3d & w1,const CartesianVector3<1> & wy)
    { return Eigen::Vector3d(w1[0],w1[1]+wy.w,w1[2]); }
    Eigen::Vector3d operator+ (const Eigen::Vector3d & w1,const CartesianVector3<2> & wz)
    { return Eigen::Vector3d(w1[0],w1[1],w1[2]+wz.w); }
  } // namespace revolute

  template<int axis> 
  struct JointRevolute {
    struct BiasZero 
    {
      operator Motion () const { return Motion::Zero(); }
    };
    friend const Motion & operator+ ( const Motion& v, const BiasZero&) { return v; }
    friend const Motion & operator+ ( const BiasZero&,const Motion& v) { return v; }

    struct MotionRevolute 
    {
      MotionRevolute()                   : w(NAN) {}
      MotionRevolute( const double & w ) : w(w)  {}
      double w;

      operator Motion() const
      { 
	return Motion(Motion::Vector3::Zero(),
		      (Motion::Vector3)typename revolute::CartesianVector3<axis>(w));
      }
    }; // struct MotionRevolute

    friend const MotionRevolute& operator+ (const MotionRevolute& m, const BiasZero&)
    { return m; }

    friend Motion operator+( const MotionRevolute& m1, const Motion& m2)
    {
      return Motion( m2.linear(),m2.angular()+typename revolute::CartesianVector3<axis>(m1.w)); 
    }    
    struct ConstraintRevolute
    { 
      template<typename D>
      MotionRevolute operator*( const Eigen::MatrixBase<D> & v ) const
      { return MotionRevolute(v[axis]); }

      Eigen::Matrix<double,6,1> se3Action(const SE3 & m) const
      { 
	Eigen::Matrix<double,6,1> res;
	res.head<3>() = m.translation().cross( m.rotation().col(axis));
	res.tail<3>() = m.rotation().col(axis);
	return res;<commit_msg>Corrected a major bug in the joint revolute (doubtlessly introduced by recent commit 941214).<commit_after>#ifndef __se3_joint_revolute_hpp__
#define __se3_joint_revolute_hpp__

#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/math/sincos.hpp"

namespace se3
{

  template<int axis> struct JointDataRevolute;
  template<int axis> struct JointModelRevolute;
  
  namespace revolute
  {
    template<int axis>
    struct CartesianVector3
    {
      double w; 
      CartesianVector3(const double & w) : w(w) {}
      CartesianVector3() : w(1) {}
      operator Eigen::Vector3d (); // { return Eigen::Vector3d(w,0,0); }
    };
    template<>CartesianVector3<0>::operator Eigen::Vector3d () { return Eigen::Vector3d(w,0,0); }
    template<>CartesianVector3<1>::operator Eigen::Vector3d () { return Eigen::Vector3d(0,w,0); }
    template<>CartesianVector3<2>::operator Eigen::Vector3d () { return Eigen::Vector3d(0,0,w); }
    Eigen::Vector3d operator+ (const Eigen::Vector3d & w1,const CartesianVector3<0> & wx)
    { return Eigen::Vector3d(w1[0]+wx.w,w1[1],w1[2]); }
    Eigen::Vector3d operator+ (const Eigen::Vector3d & w1,const CartesianVector3<1> & wy)
    { return Eigen::Vector3d(w1[0],w1[1]+wy.w,w1[2]); }
    Eigen::Vector3d operator+ (const Eigen::Vector3d & w1,const CartesianVector3<2> & wz)
    { return Eigen::Vector3d(w1[0],w1[1],w1[2]+wz.w); }
  } // namespace revolute

  template<int axis> 
  struct JointRevolute {
    struct BiasZero 
    {
      operator Motion () const { return Motion::Zero(); }
    };
    friend const Motion & operator+ ( const Motion& v, const BiasZero&) { return v; }
    friend const Motion & operator+ ( const BiasZero&,const Motion& v) { return v; }

    struct MotionRevolute 
    {
      MotionRevolute()                   : w(NAN) {}
      MotionRevolute( const double & w ) : w(w)  {}
      double w;

      operator Motion() const
      { 
	return Motion(Motion::Vector3::Zero(),
		      (Motion::Vector3)typename revolute::CartesianVector3<axis>(w));
      }
    }; // struct MotionRevolute

    friend const MotionRevolute& operator+ (const MotionRevolute& m, const BiasZero&)
    { return m; }

    friend Motion operator+( const MotionRevolute& m1, const Motion& m2)
    {
      return Motion( m2.linear(),m2.angular()+typename revolute::CartesianVector3<axis>(m1.w)); 
    }    
    struct ConstraintRevolute
    { 
      template<typename D>
      MotionRevolute operator*( const Eigen::MatrixBase<D> & v ) const
      { return MotionRevolute(v[0]); }

      Eigen::Matrix<double,6,1> se3Action(const SE3 & m) const
      { 
	Eigen::Matrix<double,6,1> res;
	res.head<3>() = m.translation().cross( m.rotation().col(axis));
	res.tail<3>() = m.rotation().col(axis);
	return res;
{'fraction_non_alphanumeric': 0.1458147421024451, 'fraction_numerical': 0.030519364626093164, 'mean_word_length': 3.818572656921754, 'pattern_counts': {'":': 0, '<': 45, '<?xml version=': 0, '>': 45, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3854741', 'n_tokens_mistral': 2019, 'n_tokens_neox': 1898, 'n_words': 378}