Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/tools/clamp-workspace.cpp	Lines:	0	110	0.0 %
Date:	2023-03-13 12:09:37	Branches:	0	239	0.0 %


/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */

#include <cmath>
#include <sot/core/clamp-workspace.hh>

using namespace std;

#include <dynamic-graph/factory.h>

using namespace dynamicgraph::sot;
using namespace dynamicgraph;

DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ClampWorkspace, "ClampWorkspace");

ClampWorkspace::ClampWorkspace(const string &fName)
    : Entity(fName),
      positionrefSIN(
          NULL, "ClampWorkspace(" + name + ")::input(double)::positionref"),
      positionSIN(NULL, "ClampWorkspace(" + name + ")::input(double)::position")

      ,
      alphaSOUT(boost::bind(&ClampWorkspace::computeOutput, this, _1, _2),
                positionrefSIN << positionSIN,
                "ClampWorkspace(" + name + ")::output(vector)::alpha")

      ,
      alphabarSOUT(boost::bind(&ClampWorkspace::computeOutputBar, this, _1, _2),
                   positionrefSIN << positionSIN,
                   "ClampWorkspace(" + name + ")::output(vector)::alphabar")

      ,
      handrefSOUT(boost::bind(&ClampWorkspace::computeRef, this, _1, _2),
                  positionrefSIN << positionSIN,
                  "ClampWorkspace(" + name + ")::output(vector)::ref")

      ,
      timeUpdate(0)

      ,
      alpha(6, 6),
      alphabar(6, 6)

      ,
      pd(3)

      ,
      beta(1),
      scale(0),
      dm_min(0.019),
      dm_max(0.025),
      dm_min_yaw(0.019),
      dm_max_yaw(0.119),
      theta_min(-30. * 3.14159 / 180.),
      theta_max(5. * 3.14159 / 180.),
      mode(1),
      frame(FRAME_POINT)

{
  alpha.setZero();
  alphabar.fill(1.);
  bounds[0] = std::make_pair(0.15, 0.5);
  bounds[1] = std::make_pair(-0.4, -0.25);
  bounds[2] = std::make_pair(0.15, 0.55);

  signalRegistration(positionrefSIN << positionSIN << alphaSOUT << alphabarSOUT
                                    << handrefSOUT);
}

void ClampWorkspace::update(int time) {
  if (time <= timeUpdate) {
    return;
  }

  alpha.setZero();
  alphabar.setIdentity();

  const MatrixHomogeneous &posref = positionrefSIN.access(time);
  const MatrixHomogeneous &pos = positionSIN.access(time);

  MatrixHomogeneous prefMw = posref.inverse(Eigen::Affine);
  prefMp = prefMw * pos;
  Vector x(prefMp.translation());

  for (int i = 0; i < 3; ++i) {
    double check_min = std::max(x(i) - bounds[i].first, 0.);
    double check_max = std::max(bounds[i].second - x(i), 0.);
    double dm = std::min(check_min, check_max);

    double Y = (dm - dm_min) / (dm_max - dm_min);
    if (Y < 0) {
      Y = 0;
    }
    if (Y > 1) {
      Y = 1;
    }

    switch (mode) {
      case 0:
        alpha(i, i) = 0;
        alphabar(i, i) = 1;
        break;
      case 1:
        alpha(i, i) = 1;
        alphabar(i, i) = 0;
        break;
      case 2:
      default:
        alpha(i, i) = 0.5 * (1 + tanh(1 / Y - 1 / (1 - Y)));
        alphabar(i, i) = 1 - alpha(i);
    }

    if (i == 2) {
      Y = (dm - dm_min_yaw) / (dm_max_yaw - dm_min_yaw);
      if (Y < 0) {
        Y = 0;
      }
      if (Y > 1) {
        Y = 1;
      }
      if (mode == 2) {
        alpha(i + 3, i + 3) = 0.5 * (1 + tanh(1 / Y - 1 / (1 - Y)));
        alphabar(i + 3, i + 3) = 1 - alpha(i + 3);
      }
    }
  }

  if (frame == FRAME_POINT) {
    MatrixHomogeneous prefMp_tmp = prefMp;
    MatrixHomogeneous pMpref = prefMp.inverse(Eigen::Affine);
    for (int i = 0; i < 3; ++i) {
      pMpref(i, 3) = 0;
      prefMp_tmp(i, 3) = 0;
    }

    MatrixTwist pTpref;
    buildFrom(pMpref, pTpref);
    MatrixTwist prefTp;
    buildFrom(prefMp_tmp, prefTp);

    Matrix tmp;
    tmp = alpha * prefTp;
    alpha = pTpref * tmp;

    tmp = alphabar * prefTp;
    alphabar = pTpref * tmp;
  }

  for (int i = 0; i < 3; ++i) {
    pd(i) = 0.5 * (bounds[i].first + bounds[i].second);
  }

  VectorRollPitchYaw rpy;
  rpy(0) = 0;
  rpy(1) = -3.14159256 / 2;
  rpy(2) = theta_min + (theta_max - theta_min) * (x(1) - bounds[1].first) /
                           (bounds[1].second - bounds[1].first);

  Eigen::Affine3d _Rd(Eigen::AngleAxisd(rpy(2), Eigen::Vector3d::UnitZ()) *
                      Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY()) *
                      Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX()));
  Rd = _Rd.linear();

  Eigen::Affine3d _tmpaffine;
  _tmpaffine = Eigen::Translation3d(pd);

  handref = (_tmpaffine * _Rd);
  timeUpdate = time;
}

Matrix &ClampWorkspace::computeOutput(Matrix &res, int time) {
  update(time);
  res = alpha;
  return res;
}

Matrix &ClampWorkspace::computeOutputBar(Matrix &res, int time) {
  update(time);
  res = alphabar;
  return res;
}

MatrixHomogeneous &ClampWorkspace::computeRef(MatrixHomogeneous &res,
                                              int time) {
  update(time);
  res = handref;
  return res;
}

void ClampWorkspace::display(std::ostream &os) const {
  os << "ClampWorkspace<" << name << ">" << endl << endl;
  os << "alpha: " << alpha << endl;
  os << "pos in ws: " << prefMp << endl;
  os << "bounds: " << bounds[0].first << " " << bounds[0].second << " "
     << bounds[1].first << " " << bounds[1].second << " " << bounds[2].first
     << " " << bounds[2].second << endl;
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Copyright 2010,
3			* François Bleibel,
4			* Olivier Stasse,
5			*
6			* CNRS/AIST
7			*
8			*/
9
10			#include <cmath>
11			#include <sot/core/clamp-workspace.hh>
12
13			using namespace std;
14
15			#include <dynamic-graph/factory.h>
16
17			using namespace dynamicgraph::sot;
18			using namespace dynamicgraph;
19
20			DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(ClampWorkspace, "ClampWorkspace");
21
22			ClampWorkspace::ClampWorkspace(const string &fName)
23			: Entity(fName),
24			positionrefSIN(
25			NULL, "ClampWorkspace(" + name + ")::input(double)::positionref"),
26			positionSIN(NULL, "ClampWorkspace(" + name + ")::input(double)::position")
27
28			,
29			alphaSOUT(boost::bind(&ClampWorkspace::computeOutput, this, _1, _2),
30			positionrefSIN << positionSIN,
31			"ClampWorkspace(" + name + ")::output(vector)::alpha")
32
33			,
34			alphabarSOUT(boost::bind(&ClampWorkspace::computeOutputBar, this, _1, _2),
35			positionrefSIN << positionSIN,
36			"ClampWorkspace(" + name + ")::output(vector)::alphabar")
37
38			,
39			handrefSOUT(boost::bind(&ClampWorkspace::computeRef, this, _1, _2),
40			positionrefSIN << positionSIN,
41			"ClampWorkspace(" + name + ")::output(vector)::ref")
42
43			,
44			timeUpdate(0)
45
46			,
47			alpha(6, 6),
48			alphabar(6, 6)
49
50			,
51			pd(3)
52
53			,
54			beta(1),
55			scale(0),
56			dm_min(0.019),
57			dm_max(0.025),
58			dm_min_yaw(0.019),
59			dm_max_yaw(0.119),
60			theta_min(-30. * 3.14159 / 180.),
61			theta_max(5. * 3.14159 / 180.),
62			mode(1),
63			frame(FRAME_POINT)
64
65			{
66			alpha.setZero();
67			alphabar.fill(1.);
68			bounds[0] = std::make_pair(0.15, 0.5);
69			bounds[1] = std::make_pair(-0.4, -0.25);
70			bounds[2] = std::make_pair(0.15, 0.55);
71
72			signalRegistration(positionrefSIN << positionSIN << alphaSOUT << alphabarSOUT
73			<< handrefSOUT);
74			}
75
76			void ClampWorkspace::update(int time) {
77			if (time <= timeUpdate) {
78			return;
79			}
80
81			alpha.setZero();
82			alphabar.setIdentity();
83
84			const MatrixHomogeneous &posref = positionrefSIN.access(time);
85			const MatrixHomogeneous &pos = positionSIN.access(time);
86
87			MatrixHomogeneous prefMw = posref.inverse(Eigen::Affine);
88			prefMp = prefMw * pos;
89			Vector x(prefMp.translation());
90
91			for (int i = 0; i < 3; ++i) {
92			double check_min = std::max(x(i) - bounds[i].first, 0.);
93			double check_max = std::max(bounds[i].second - x(i), 0.);
94			double dm = std::min(check_min, check_max);
95
96			double Y = (dm - dm_min) / (dm_max - dm_min);
97			if (Y < 0) {
98			Y = 0;
99			}
100			if (Y > 1) {
101			Y = 1;
102			}
103
104			switch (mode) {
105			case 0:
106			alpha(i, i) = 0;
107			alphabar(i, i) = 1;
108			break;
109			case 1:
110			alpha(i, i) = 1;
111			alphabar(i, i) = 0;
112			break;
113			case 2:
114			default:
115			alpha(i, i) = 0.5 * (1 + tanh(1 / Y - 1 / (1 - Y)));
116			alphabar(i, i) = 1 - alpha(i);
117			}
118
119			if (i == 2) {
120			Y = (dm - dm_min_yaw) / (dm_max_yaw - dm_min_yaw);
121			if (Y < 0) {
122			Y = 0;
123			}
124			if (Y > 1) {
125			Y = 1;
126			}
127			if (mode == 2) {
128			alpha(i + 3, i + 3) = 0.5 * (1 + tanh(1 / Y - 1 / (1 - Y)));
129			alphabar(i + 3, i + 3) = 1 - alpha(i + 3);
130			}
131			}
132			}
133
134			if (frame == FRAME_POINT) {
135			MatrixHomogeneous prefMp_tmp = prefMp;
136			MatrixHomogeneous pMpref = prefMp.inverse(Eigen::Affine);
137			for (int i = 0; i < 3; ++i) {
138			pMpref(i, 3) = 0;
139			prefMp_tmp(i, 3) = 0;
140			}
141
142			MatrixTwist pTpref;
143			buildFrom(pMpref, pTpref);
144			MatrixTwist prefTp;
145			buildFrom(prefMp_tmp, prefTp);
146
147			Matrix tmp;
148			tmp = alpha * prefTp;
149			alpha = pTpref * tmp;
150
151			tmp = alphabar * prefTp;
152			alphabar = pTpref * tmp;
153			}
154
155			for (int i = 0; i < 3; ++i) {
156			pd(i) = 0.5 * (bounds[i].first + bounds[i].second);
157			}
158
159			VectorRollPitchYaw rpy;
160			rpy(0) = 0;
161			rpy(1) = -3.14159256 / 2;
162			rpy(2) = theta_min + (theta_max - theta_min) * (x(1) - bounds[1].first) /
163			(bounds[1].second - bounds[1].first);
164
165			Eigen::Affine3d _Rd(Eigen::AngleAxisd(rpy(2), Eigen::Vector3d::UnitZ()) *
166			Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY()) *
167			Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX()));
168			Rd = _Rd.linear();
169
170			Eigen::Affine3d _tmpaffine;
171			_tmpaffine = Eigen::Translation3d(pd);
172
173			handref = (_tmpaffine * _Rd);
174			timeUpdate = time;
175			}
176
177			Matrix &ClampWorkspace::computeOutput(Matrix &res, int time) {
178			update(time);
179			res = alpha;
180			return res;
181			}
182
183			Matrix &ClampWorkspace::computeOutputBar(Matrix &res, int time) {
184			update(time);
185			res = alphabar;
186			return res;
187			}
188
189			MatrixHomogeneous &ClampWorkspace::computeRef(MatrixHomogeneous &res,
190			int time) {
191			update(time);
192			res = handref;
193			return res;
194			}
195
196			void ClampWorkspace::display(std::ostream &os) const {
197			os << "ClampWorkspace<" << name << ">" << endl << endl;
198			os << "alpha: " << alpha << endl;
199			os << "pos in ws: " << prefMp << endl;
200			os << "bounds: " << bounds[0].first << " " << bounds[0].second << " "
201			<< bounds[1].first << " " << bounds[1].second << " " << bounds[2].first
202			<< " " << bounds[2].second << endl;
203			}