neck-limitation.cpp

Go to the documentation of this file.

/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */
 
#include <dynamic-graph/pool.h>
 
#include <sot/core/debug.hh>
#include <sot/core/exception-tools.hh>
#include <sot/core/factory.hh>
#include <sot/core/neck-limitation.hh>
#include <sot/core/sot.hh>
 
using namespace dynamicgraph::sot;
using namespace dynamicgraph;
 
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(NeckLimitation, "NeckLimitation");
 
const double NeckLimitation::COEFF_LINEAR_DEFAULT = -25.0 / 42.0;
const double NeckLimitation::COEFF_AFFINE_DEFAULT = 0.6981;  // 40DG
const double NeckLimitation::SIGN_TILT_DEFAULT = 1;
const std::size_t NeckLimitation::PAN_RANK_DEFAULT = 14;
const std::size_t NeckLimitation::TILT_RANK_DEFAULT = 15;
 
NeckLimitation::NeckLimitation(const std::string &name)
    : Entity(name),
      panRank(PAN_RANK_DEFAULT),
      tiltRank(TILT_RANK_DEFAULT),
      coeffLinearPan(COEFF_LINEAR_DEFAULT),
      coeffAffinePan(COEFF_AFFINE_DEFAULT),
      signTilt(SIGN_TILT_DEFAULT)
 
      ,
      jointSIN(NULL, "NeckLimitation(" + name + ")::input(vector)::joint"),
      jointSOUT(
          boost::bind(&NeckLimitation::computeJointLimitation, this, _1, _2),
          jointSIN,
          "NeckLimitation(" + name + ")::output(dummy)::jointLimited") {
  sotDEBUGIN(5);
 
  signalRegistration(jointSIN << jointSOUT);
 
  sotDEBUGOUT(5);
}
 
NeckLimitation::~NeckLimitation(void) {
  sotDEBUGIN(5);
 
  sotDEBUGOUT(5);
  return;
}
 
/* --- SIGNALS -------------------------------------------------------------- */
/* --- SIGNALS -------------------------------------------------------------- */
/* --- SIGNALS -------------------------------------------------------------- */
 
dynamicgraph::Vector &NeckLimitation::computeJointLimitation(
    dynamicgraph::Vector &jointLimited, const sigtime_t &timeSpec) {
  sotDEBUGIN(15);
 
  const dynamicgraph::Vector &joint = jointSIN(timeSpec);
  jointLimited = joint;
 
  const double &pan = joint(panRank);
  const double &tilt = joint(tiltRank);
  double &panLimited = jointLimited(panRank);
  double &tiltLimited = jointLimited(tiltRank);
 
  if (fabs(pan) < 1e-3)  // pan == 0
  {
    sotDEBUG(15) << "Pan = 0" << std::endl;
    if (tilt * signTilt > coeffAffinePan) {
      tiltLimited = coeffAffinePan;
    } else {
      tiltLimited = tilt;
    }
    panLimited = pan;
  } else if (pan > 0) {
    sotDEBUG(15) << "Pan > 0" << std::endl;
    if (signTilt * tilt > (pan * coeffLinearPan + coeffAffinePan)) {
      // Orthogonal projection .
      //          if( (tilt-coeffAffinePan)*coeffLinearPan<-1*pan )
      //            {
      //              panLimited=0; tiltLimited=coeffAffinePan;
      //            }
      //          else
      //            {
      //              double tmp = 1/(1+coeffLinearPan*coeffLinearPan);
      //              double tmp2=pan+coeffLinearPan*tilt;
 
      //              panLimited=(tmp2-coeffAffinePan*coeffLinearPan)*tmp;
      //              tiltLimited=(coeffLinearPan*tmp2+coeffAffinePan)*tmp;
      //            }
      tiltLimited = (pan * coeffLinearPan + coeffAffinePan);
      panLimited = pan;
    } else {
      tiltLimited = tilt;
      panLimited = pan;
    }
  } else  // pan<0
  {
    sotDEBUG(15) << "Pan < 0" << std::endl;
    sotDEBUG(15) << tilt - coeffAffinePan << "<?" << (-1 * pan * coeffLinearPan)
                 << std::endl;
    if (tilt * signTilt > (-pan * coeffLinearPan + coeffAffinePan)) {
      //          sotDEBUG(15) << "Below" << std::endl;
      //          if( (tilt-coeffAffinePan)*coeffLinearPan<pan )
      //            {
      //              sotDEBUG(15) << "Proj on 0" << std::endl;
      //              panLimited=0; tiltLimited=coeffAffinePan;
      //            }
      //          else
      //            {
      //              double tmp = 1/(1+coeffLinearPan*coeffLinearPan);
      //              double tmp2=pan-coeffLinearPan*tilt;
 
      //              panLimited=(tmp2+coeffAffinePan*coeffLinearPan)*tmp;
      //              tiltLimited=(-coeffLinearPan*tmp2+coeffAffinePan)*tmp;
      //            }
      tiltLimited = (-pan * coeffLinearPan + coeffAffinePan);
      panLimited = pan;
    } else {
      tiltLimited = tilt;
      panLimited = pan;
    }
  }
 
  sotDEBUGOUT(15);
  return jointLimited;
}
 
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
/* --- PARAMS --------------------------------------------------------------- */
 
void NeckLimitation::display(std::ostream &os) const {
  os << "NeckLimitation " << getName() << "." << std::endl;
}