feature-posture.cpp 4.51 KB
Newer Older
1
2
3
4
// Copyright 2010, François Bleibel, Thomas Moulard, Olivier Stasse,
// JRL, CNRS/AIST.

#include <boost/assign/list_of.hpp>
5
#include <dynamic-graph/command-bind.h>
6
7
#include <dynamic-graph/factory.h>
#include <dynamic-graph/pool.h>
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
8
#include <string>
9

10
#include <boost/numeric/conversion/cast.hpp>
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
11
#include <sot/core/feature-posture.hh>
12
13
14
15
16
namespace dg = ::dynamicgraph;

using dynamicgraph::sot::FeatureAbstract;

namespace dynamicgraph {
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
namespace sot {
using command::Command;
using command::Value;

class FeaturePosture::SelectDof : public Command {
public:
  virtual ~SelectDof() {}
  SelectDof(FeaturePosture &entity, const std::string &docstring)
      : Command(entity, boost::assign::list_of(Value::UNSIGNED)(Value::BOOL),
                docstring) {}
  virtual Value doExecute() {
    FeaturePosture &feature = static_cast<FeaturePosture &>(owner());
    std::vector<Value> values = getParameterValues();
    unsigned int dofId = values[0].value();
    bool control = values[1].value();
    feature.selectDof(dofId, control);
    return Value();
  }
}; // class SelectDof

FeaturePosture::FeaturePosture(const std::string &name)
    : FeatureAbstract(name),
      state_(NULL, "FeaturePosture(" + name + ")::input(Vector)::state"),
      posture_(0, "FeaturePosture(" + name + ")::input(Vector)::posture"),
      postureDot_(0, "FeaturePosture(" + name + ")::input(Vector)::postureDot"),
42
      activeDofs_(), nbActiveDofs_(0) {
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
43
44
45
  signalRegistration(state_ << posture_ << postureDot_);

  errorSOUT.addDependency(state_);
46
  jacobianSOUT.setConstant(Matrix());
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80

  std::string docstring;
  docstring = "    \n"
              "    Select degree of freedom to control\n"
              "    \n"
              "      input:\n"
              "        - positive integer: rank of degree of freedom,\n"
              "        - boolean: whether to control the selected degree of "
              "freedom.\n"
              "    \n"
              "      Note: rank should be more than 5 since posture is "
              "independent\n"
              "        from freeflyer position.\n"
              "    \n";
  addCommand("selectDof", new SelectDof(*this, docstring));
}

FeaturePosture::~FeaturePosture() {}

unsigned int &FeaturePosture::getDimension(unsigned int &res, int) {
  res = static_cast<unsigned int>(nbActiveDofs_);
  return res;
}

dg::Vector &FeaturePosture::computeError(dg::Vector &res, int t) {
  const dg::Vector &state = state_.access(t);
  const dg::Vector &posture = posture_.access(t);

  res.resize(nbActiveDofs_);
  std::size_t index = 0;
  for (std::size_t i = 0; i < activeDofs_.size(); ++i) {
    if (activeDofs_[i]) {
      res(index) = state(i) - posture(i);
      index++;
81
    }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
82
83
84
85
  }
  return res;
}

86
dg::Matrix &FeaturePosture::computeJacobian(dg::Matrix &, int) {
Olivier Stasse's avatar
Olivier Stasse committed
87
88
  throw std::runtime_error("jacobian signal should be constant."
                           " This function should never be called");
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
}

dg::Vector &FeaturePosture::computeErrorDot(dg::Vector &res, int t) {
  const Vector &postureDot = postureDot_.access(t);

  res.resize(nbActiveDofs_);
  std::size_t index = 0;
  for (std::size_t i = 0; i < activeDofs_.size(); ++i) {
    if (activeDofs_[i])
      res(index++) = -postureDot(i);
  }
  return res;
}

void FeaturePosture::selectDof(unsigned dofId, bool control) {
  const Vector &state = state_.accessCopy();
  const Vector &posture = posture_.accessCopy();
  std::size_t dim(state.size());

  if (dim != (std::size_t)posture.size()) {
    throw std::runtime_error("Posture and State should have same dimension.");
  }
  // If activeDof_ vector not initialized, initialize it
  if (activeDofs_.size() != dim) {
    activeDofs_ = std::vector<bool>(dim, false);
    nbActiveDofs_ = 0;
  }

  // Check that selected dof id is valid
118
  if (dofId >= dim) {
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
119
    std::ostringstream oss;
Maximilien Naveau's avatar
Maximilien Naveau committed
120
121
    oss << "dof id should less than state dimension: " << dim << ". Received "
        << dofId << ".";
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
122
123
124
125
126
127
128
    throw ExceptionAbstract(ExceptionAbstract::TOOLS, oss.str());
  }

  if (control) {
    if (!activeDofs_[dofId]) {
      activeDofs_[dofId] = true;
      nbActiveDofs_++;
129
    }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
130
131
132
133
  } else { // control = false
    if (activeDofs_[dofId]) {
      activeDofs_[dofId] = false;
      nbActiveDofs_--;
134
    }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
135
136
  }
  // recompute jacobian
Olivier Stasse's avatar
Olivier Stasse committed
137
  Matrix J(Matrix::Zero(nbActiveDofs_, dim));
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
138
139
140
141

  std::size_t index = 0;
  for (std::size_t i = 0; i < activeDofs_.size(); ++i) {
    if (activeDofs_[i]) {
142
      J(index, i) = 1;
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
143
      index++;
144
    }
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
145
  }
146
147

  jacobianSOUT.setConstant(J);
Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
148
}
149

Guilhem Saurel's avatar
Format    
Guilhem Saurel committed
150
151
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(FeaturePosture, "FeaturePosture");
} // namespace sot
152
} // namespace dynamicgraph