GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/zmp-from-forces.h Lines: 0 49 0.0 %
Date: 2023-01-28 11:04:48 Branches: 0 70 0.0 %

Line Branch Exec Source
1
/*
2
 * Copyright 2012,
3
 * Florent Lamiraux
4
 *
5
 * CNRS
6
 *
7
 */
8
9
#include <dynamic-graph/entity.h>
10
#include <dynamic-graph/factory.h>
11
#include <dynamic-graph/linear-algebra.h>
12
#include <dynamic-graph/signal-ptr.h>
13
#include <dynamic-graph/signal-time-dependent.h>
14
15
#include <sot/core/matrix-geometry.hh>
16
#include <sstream>
17
#include <vector>
18
19
namespace sot {
20
namespace dynamic {
21
using dynamicgraph::Entity;
22
using dynamicgraph::SignalPtr;
23
using dynamicgraph::SignalTimeDependent;
24
using dynamicgraph::Vector;
25
using dynamicgraph::sot::MatrixHomogeneous;
26
27
class ZmpFromForces : public Entity {
28
  DYNAMIC_GRAPH_ENTITY_DECL();
29
30
 public:
31
  static const unsigned int footNumber = 2;
32
  ZmpFromForces(const std::string& name)
33
      : Entity(name), zmpSOUT_(CLASS_NAME + "::output(Vector)::zmp") {
34
    zmpSOUT_.setFunction(boost::bind(&ZmpFromForces::computeZmp, this, _1, _2));
35
    signalRegistration(zmpSOUT_);
36
    for (unsigned int i = 0; i < footNumber; i++) {
37
      std::ostringstream forceName, positionName;
38
      forceName << CLASS_NAME << "::input(vector6)::force_" << i;
39
      positionName << CLASS_NAME << "::input(MatrixHomo)::sensorPosition_" << i;
40
      forcesSIN_[i] = new SignalPtr<Vector, int>(0, forceName.str());
41
      sensorPositionsSIN_[i] =
42
          new SignalPtr<MatrixHomogeneous, int>(0, positionName.str());
43
      signalRegistration(*forcesSIN_[i]);
44
      signalRegistration(*sensorPositionsSIN_[i]);
45
      zmpSOUT_.addDependency(*forcesSIN_[i]);
46
      zmpSOUT_.addDependency(*sensorPositionsSIN_[i]);
47
    }
48
  }
49
  virtual std::string getDocString() const {
50
    std::string docstring =
51
        "Compute ZMP from force sensor measures and positions\n"
52
        "\n"
53
        "  Takes 4 signals as input:\n"
54
        "    - force_0: wrench measured by force sensor 0 as a 6 dimensional "
55
        "vector\n"
56
        "    - force_0: wrench measured by force sensor 1 as a 6 dimensional "
57
        "vector\n"
58
        "    - sensorPosition_0: position of force sensor 0\n"
59
        "    - sensorPosition_1: position of force sensor 1\n"
60
        "  \n"
61
        "  compute the Zero Momentum Point of the contact forces as measured "
62
        "by the \n"
63
        "  input signals under the asumptions that the contact points between "
64
        "the\n"
65
        "  robot and the environment are located in the same horizontal "
66
        "plane.\n";
67
    return docstring;
68
  }
69
70
 private:
71
  Vector& computeZmp(Vector& zmp, int time) {
72
    double fnormal = 0;
73
    double sumZmpx = 0;
74
    double sumZmpy = 0;
75
    double sumZmpz = 0;
76
    zmp.resize(3);
77
78
    for (unsigned int i = 0; i < footNumber; ++i) {
79
      const Vector& f = forcesSIN_[i]->access(time);
80
      // Check that force is of dimension 6
81
      if (f.size() != 6) {
82
        zmp.fill(0.);
83
        return zmp;
84
      }
85
      const MatrixHomogeneous& M = sensorPositionsSIN_[i]->access(time);
86
      double fx = M(0, 0) * f(0) + M(0, 1) * f(1) + M(0, 2) * f(2);
87
      double fy = M(1, 0) * f(0) + M(1, 1) * f(1) + M(1, 2) * f(2);
88
      double fz = M(2, 0) * f(0) + M(2, 1) * f(1) + M(2, 2) * f(2);
89
90
      if (fz > 0) {
91
        double Mx = M(0, 0) * f(3) + M(0, 1) * f(4) + M(0, 2) * f(5) +
92
                    M(1, 3) * fz - M(2, 3) * fy;
93
        double My = M(1, 0) * f(3) + M(1, 1) * f(4) + M(1, 2) * f(5) +
94
                    M(2, 3) * fx - M(0, 3) * fz;
95
        fnormal += fz;
96
        sumZmpx -= My;
97
        sumZmpy += Mx;
98
        sumZmpz += fz * M(2, 3);
99
      }
100
    }
101
    if (fnormal != 0) {
102
      zmp(0) = sumZmpx / fnormal;
103
      zmp(1) = sumZmpy / fnormal;
104
      zmp(2) = sumZmpz / fnormal;
105
    } else {
106
      zmp.fill(0.);
107
    }
108
    return zmp;
109
  }
110
  // Force as measured by force sensor on the foot
111
  SignalPtr<Vector, int>* forcesSIN_[footNumber];
112
  SignalPtr<MatrixHomogeneous, int>* sensorPositionsSIN_[footNumber];
113
  SignalTimeDependent<Vector, int> zmpSOUT_;
114
};  // class ZmpFromForces
115
}  // namespace dynamic
116
}  // namespace sot