neck-limitation.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2010,
3  * François Bleibel,
4  * Olivier Stasse,
5  *
6  * CNRS/AIST
7  *
8  */
9 
10 #include <dynamic-graph/pool.h>
11 
12 #include <sot/core/debug.hh>
14 #include <sot/core/factory.hh>
16 #include <sot/core/sot.hh>
17 
18 using namespace dynamicgraph::sot;
19 using namespace dynamicgraph;
20 
22 
23 const double NeckLimitation::COEFF_LINEAR_DEFAULT = -25.0 / 42.0;
24 const double NeckLimitation::COEFF_AFFINE_DEFAULT = 0.6981; // 40DG
25 const double NeckLimitation::SIGN_TILT_DEFAULT = 1;
26 const unsigned int NeckLimitation::PAN_RANK_DEFAULT = 14;
27 const unsigned int NeckLimitation::TILT_RANK_DEFAULT = 15;
28 
30  : Entity(name),
31  panRank(PAN_RANK_DEFAULT),
32  tiltRank(TILT_RANK_DEFAULT),
33  coeffLinearPan(COEFF_LINEAR_DEFAULT),
34  coeffAffinePan(COEFF_AFFINE_DEFAULT),
35  signTilt(SIGN_TILT_DEFAULT)
36 
37  ,
38  jointSIN(NULL, "NeckLimitation(" + name + ")::input(vector)::joint"),
39  jointSOUT(
40  boost::bind(&NeckLimitation::computeJointLimitation, this, _1, _2),
41  jointSIN,
42  "NeckLimitation(" + name + ")::output(dummy)::jointLimited") {
43  sotDEBUGIN(5);
44 
46 
47  sotDEBUGOUT(5);
48 }
49 
51  sotDEBUGIN(5);
52 
53  sotDEBUGOUT(5);
54  return;
55 }
56 
57 /* --- SIGNALS -------------------------------------------------------------- */
58 /* --- SIGNALS -------------------------------------------------------------- */
59 /* --- SIGNALS -------------------------------------------------------------- */
60 
62  dynamicgraph::Vector &jointLimited, const int &timeSpec) {
63  sotDEBUGIN(15);
64 
65  const dynamicgraph::Vector &joint = jointSIN(timeSpec);
66  jointLimited = joint;
67 
68  const double &pan = joint(panRank);
69  const double &tilt = joint(tiltRank);
70  double &panLimited = jointLimited(panRank);
71  double &tiltLimited = jointLimited(tiltRank);
72 
73  if (fabs(pan) < 1e-3) // pan == 0
74  {
75  sotDEBUG(15) << "Pan = 0" << std::endl;
76  if (tilt * signTilt > coeffAffinePan) {
77  tiltLimited = coeffAffinePan;
78  } else {
79  tiltLimited = tilt;
80  }
81  panLimited = pan;
82  } else if (pan > 0) {
83  sotDEBUG(15) << "Pan > 0" << std::endl;
84  if (signTilt * tilt > (pan * coeffLinearPan + coeffAffinePan)) {
85  // Orthogonal projection .
86  // if( (tilt-coeffAffinePan)*coeffLinearPan<-1*pan )
87  // {
88  // panLimited=0; tiltLimited=coeffAffinePan;
89  // }
90  // else
91  // {
92  // double tmp = 1/(1+coeffLinearPan*coeffLinearPan);
93  // double tmp2=pan+coeffLinearPan*tilt;
94 
95  // panLimited=(tmp2-coeffAffinePan*coeffLinearPan)*tmp;
96  // tiltLimited=(coeffLinearPan*tmp2+coeffAffinePan)*tmp;
97  // }
98  tiltLimited = (pan * coeffLinearPan + coeffAffinePan);
99  panLimited = pan;
100  } else {
101  tiltLimited = tilt;
102  panLimited = pan;
103  }
104  } else // pan<0
105  {
106  sotDEBUG(15) << "Pan < 0" << std::endl;
107  sotDEBUG(15) << tilt - coeffAffinePan << "<?" << (-1 * pan * coeffLinearPan)
108  << std::endl;
109  if (tilt * signTilt > (-pan * coeffLinearPan + coeffAffinePan)) {
110  // sotDEBUG(15) << "Below" << std::endl;
111  // if( (tilt-coeffAffinePan)*coeffLinearPan<pan )
112  // {
113  // sotDEBUG(15) << "Proj on 0" << std::endl;
114  // panLimited=0; tiltLimited=coeffAffinePan;
115  // }
116  // else
117  // {
118  // double tmp = 1/(1+coeffLinearPan*coeffLinearPan);
119  // double tmp2=pan-coeffLinearPan*tilt;
120 
121  // panLimited=(tmp2+coeffAffinePan*coeffLinearPan)*tmp;
122  // tiltLimited=(-coeffLinearPan*tmp2+coeffAffinePan)*tmp;
123  // }
124  tiltLimited = (-pan * coeffLinearPan + coeffAffinePan);
125  panLimited = pan;
126  } else {
127  tiltLimited = tilt;
128  panLimited = pan;
129  }
130  }
131 
132  sotDEBUGOUT(15);
133  return jointLimited;
134 }
135 
136 /* --- PARAMS --------------------------------------------------------------- */
137 /* --- PARAMS --------------------------------------------------------------- */
138 /* --- PARAMS --------------------------------------------------------------- */
139 
140 void NeckLimitation::display(std::ostream &os) const {
141  os << "NeckLimitation " << getName() << "." << std::endl;
142 }
Eigen::VectorXd Vector
dynamicgraph::SignalTimeDependent< dynamicgraph::Vector, int > jointSOUT
static const double SIGN_TILT_DEFAULT
NeckLimitation(const std::string &name)
void signalRegistration(const SignalArray< int > &signals)
#define sotDEBUGOUT(level)
Definition: debug.hh:212
static const unsigned int TILT_RANK_DEFAULT
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePosture, "FeaturePosture")
#define sotDEBUGIN(level)
Definition: debug.hh:211
static const double COEFF_LINEAR_DEFAULT
virtual void display(std::ostream &os) const
static const double COEFF_AFFINE_DEFAULT
#define sotDEBUG(level)
Definition: debug.hh:165
dynamicgraph::Vector & computeJointLimitation(dynamicgraph::Vector &jointLimited, const int &timeSpec)
const std::string & getName() const
static const unsigned int PAN_RANK_DEFAULT
dynamicgraph::SignalPtr< dynamicgraph::Vector, int > jointSIN


sot-core
Author(s): Olivier Stasse, ostasse@laas.fr
autogenerated on Wed Jun 21 2023 02:51:26