Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/step-computer-force.cpp	Lines:	0	172	0.0 %
Date:	2023-06-05 08:59:09	Branches:	0	402	0.0 %


/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 * Copyright Projet JRL-Japan, 2007
 *+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * File:      StepComputer.cpp
 * Project:   SOT
 * Author:    Paul Evrard, Nicolas Mansard
 *
 * Version control
 * ===============
 *
 *  $Id$
 *
 * Description
 * ============
 *
 *
 * ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/

#include <time.h>

#include <cmath>
#ifndef WIN32
#include <sys/time.h>

#include <iostream>
#else
#include <Winsock2.h>

#include <jrl/mal/boost.hh>
#include <sot/core/utils-windows.hh>
#endif /*WIN32*/

#include <dynamic-graph/factory.h>
#include <dynamic-graph/pool.h>
#include <sot/pattern-generator/exception-pg.h>
#include <sot/pattern-generator/step-checker.h>
#include <sot/pattern-generator/step-computer-force.h>
#include <sot/pattern-generator/step-queue.h>

#include <sot/core/debug.hh>
#include <sot/core/macros-signal.hh>
#include <sot/core/matrix-geometry.hh>

namespace dynamicgraph {
namespace sot {

DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(StepComputerForce, "StepComputerForce");

StepComputerForce::StepComputerForce(const std::string &name)
    : Entity(name),
      waistMlhandSIN(
          NULL, "StepComputerForce(" + name + ")::input(vector)::waistMlhand")

      ,
      waistMrhandSIN(
          NULL, "StepComputerForce(" + name + ")::input(vector)::waistMrhand"),
      referencePositionWaistSIN(
          NULL, "StepComputerForce(" + name + ")::input(vector)::posrefwaist")

      ,
      stiffnessSIN(NULL,
                   "StepComputerForce(" + name + ")::input(vector)::stiffness")

      ,
      velocitySIN(NULL,
                  "StepComputerForce(" + name + ")::input(vector)::velocity")

      ,
      contactFootSIN(
          NULL, "StepComputerForce(" + name + ")::input(uint)::contactfoot")

      ,
      displacementSOUT(
          boost::bind(&StepComputerForce::computeDisplacement, this, _1, _2),
          referencePositionWaistSIN,
          "StepComputerForce(" + name + ")::output(vector)::displacement")

      ,
      forceSOUT(boost::bind(&StepComputerForce::computeForce, this, _1, _2),
                displacementSOUT,
                "StepComputerForce(" + name + ")::output(vector)::force")

      ,
      forceLhandSOUT(
          boost::bind(&StepComputerForce::computeForceL, this, _1, _2),
          waistMlhandSIN << referencePositionWaistSIN << forceSOUT,
          "StepComputerForce(" + name + ")::output(vector)::forceL")

      ,
      forceRhandSOUT(
          boost::bind(&StepComputerForce::computeForceR, this, _1, _2),
          waistMrhandSIN << referencePositionWaistSIN << forceSOUT,
          "StepComputerForce(" + name + ")::output(vector)::forceR"),
      waMref0(),
      twoHandObserver(0x0),
      checker(),
      logChanges("/tmp/stepcomp_changes.dat"),
      logPreview("/tmp/stepcomp_preview.dat") {
  sotDEBUGIN(5);

  signalRegistration(referencePositionWaistSIN
                     << contactFootSIN << waistMlhandSIN << waistMrhandSIN
                     << stiffnessSIN << velocitySIN << displacementSOUT
                     << forceSOUT << forceLhandSOUT << forceRhandSOUT);

  sotDEBUGOUT(5);
}

void StepComputerForce::nextStep(StepQueue &queue, int timeCurr) {
  // Introduce new step at the end of the preview window.
  if (queue.getLastStep().contact == CONTACT_LEFT_FOOT) {
    queue.pushStep(0., -queue.getZeroStepPosition(), 0.);
    logPreview << timeCurr << " " << 0 << " " << -queue.getZeroStepPosition()
               << " " << 0 << std::endl;
  } else {
    queue.pushStep(0., queue.getZeroStepPosition(), 0.);
    logPreview << timeCurr << " " << 0 << " " << queue.getZeroStepPosition()
               << " " << 0 << std::endl;
  }
}

Vector &StepComputerForce::computeDisplacement(Vector &res, int timeCurr) {
  if (!twoHandObserver) {
    std::cerr << "Observer not set" << std::endl;
    res.resize(3);
    res.setZero();
    return res;
  }

  // transformation from ref0 to ref.

  const MatrixHomogeneous &waMref = referencePositionWaistSIN.access(timeCurr);
  MatrixHomogeneous ref0Mwa;
  ref0Mwa = waMref0.inverse();
  MatrixHomogeneous ref0Mref;
  ref0Mref = ref0Mwa * waMref;

  // extract the translation part and express it in the waist frame.

  Vector t_ref0(3);
  t_ref0 = ref0Mref.translation();
  MatrixRotation waRref0;
  waRref0 = waMref0.linear();
  Vector t_wa = waRref0 * t_ref0;

  // compute the rotation that transforms ref0 into ref,
  // express it in the waist frame. Then get the associated
  // yaw (rot around z).

  MatrixRotation ref0Rwa;
  ref0Rwa = waRref0.transpose();
  MatrixRotation ref0Rref;
  ref0Rref = ref0Mref.linear();
  MatrixRotation tmp;
  tmp = ref0Rref * ref0Rwa;
  MatrixRotation Rref;
  Rref = waRref0 * tmp;
  VectorRollPitchYaw rpy;
  rpy = (Rref.eulerAngles(2, 1, 0)).reverse();

  // store the result.

  res.resize(3);
  res = t_wa;
  res(2) = rpy(2);

  return res;
}

Vector &StepComputerForce::computeForce(Vector &res, int timeCurr) {
  const Vector &dx = displacementSOUT.access(timeCurr);
  const Vector &K = stiffnessSIN.access(timeCurr);

  if ((dx.size() != 3) || (K.size() != 3) || (dx.size() != K.size())) {
    res.resize(3);
    res.setZero();
  } else {
    res = K * dx;
  }

  return res;
}

Vector &StepComputerForce::computeHandForce(Vector &res,
                                            const MatrixHomogeneous &waMh,
                                            const MatrixHomogeneous &waMref,
                                            const Vector &F) {
  if (F.size() != 3) {
    res.resize(6);
    res.fill(0.);
    return res;
  }

  Vector pref(3);
  pref = waMref.translation();
  Vector ph(3);
  ph = waMh.translation();

  Eigen::Vector3d OA;
  OA(0) = ph(0) - pref(0);
  OA(1) = ph(1) - pref(1);
  OA(2) = 0;

  Eigen::Vector3d tau;
  tau.setZero();
  tau(2) = -F(2);

  Eigen::Vector3d tauOA;
  tauOA = tau.cross(OA);
  double ntauOA = tauOA.norm();
  double nOA = OA.norm();
  double L = 2 * ntauOA * nOA;

  if (L < 1e-12) {
    tauOA.fill(0);
  } else {
    tauOA = tauOA * (1. / L);
  }

  Vector tmp(6);
  tmp.setZero();
  tmp(0) = tauOA(0) - F(0);
  tmp(1) = tauOA(1) - F(1);

  MatrixHomogeneous H;
  H = waMh.inverse();
  for (int i = 0; i < 3; ++i) {
    H(i, 3) = 0;
  }
  MatrixTwist V;
  buildFrom(H, V);
  res = V * tmp;

  return res;
}

Vector &StepComputerForce::computeForceL(Vector &res, int timeCurr) {
  const MatrixHomogeneous &waMlh = waistMlhandSIN.access(timeCurr);
  const MatrixHomogeneous &waMref = referencePositionWaistSIN.access(timeCurr);
  const Vector &F = forceSOUT.access(timeCurr);

  return computeHandForce(res, waMlh, waMref, F);
}

Vector &StepComputerForce::computeForceR(Vector &res, int timeCurr) {
  const MatrixHomogeneous &waMrh = waistMrhandSIN.access(timeCurr);
  const MatrixHomogeneous &waMref = referencePositionWaistSIN.access(timeCurr);
  const Vector &F = forceSOUT.access(timeCurr);

  return computeHandForce(res, waMrh, waMref, F);
}

void StepComputerForce::changeFirstStep(StepQueue &queue, int timeCurr) {
  const Vector &v = velocitySIN.access(timeCurr);
  unsigned sfoot = contactFootSIN.access(timeCurr);

  double y_default = 0;
  if (sfoot != 1) {  // --- left foot support ---
    y_default = -0.19;
  } else {  // -- right foot support ---
    y_default = 0.19;
  }

  // The clipping function expects the x-y coordinates of the
  // destination fly foot in the support foot frame.

  double x = v(0), y = v(1);
  double theta = v(2) * 180 / 3.14159265;

  if (std::abs(x) < 0.03) {
    x = 0;
  }
  if (std::abs(y) < 0.03) {
    y = 0;
  }
  if (std::abs(theta) < 2) {
    theta = 0;
  }

  y += y_default;

  const double THETA_MAX = 9.;
  if (theta < -THETA_MAX) {
    theta = -THETA_MAX;
  }
  if (theta > THETA_MAX) {
    theta = THETA_MAX;
  }

  double nx = 0, ny = 0;
  if (sfoot != 1) {  // left foot support phase
    if (y > 0) {
      y = -0.001;
    }
  } else {
    if (y < 0) {
      y = 0.001;
    }
  }

  checker.clipStep(x, y, nx, ny);

  // Log x-y values before and after clipping

  logChanges << timeCurr << " " << x << " " << y << " " << nx << " " << ny
             << " ";

  // The coordinates must be expressed in the destination foot frame.
  // See the technical report of Olivier Stasse for more details,
  // on top of page 79.

  double theta_rad = 3.14159265 * theta / 180.;
  double ctheta = cos(theta_rad);
  double stheta = sin(theta_rad);

  x = nx * ctheta + ny * stheta;
  y = -nx * stheta + ny * ctheta;

  queue.changeFirstStep(x, y, theta);

  // Log the step

  logChanges << x << " " << y << " " << theta << std::endl;
}

void StepComputerForce::thisIsZero() {
  sotDEBUGIN(15);

  waMref0 = referencePositionWaistSIN.accessCopy();

  sotDEBUGOUT(15);
}

void StepComputerForce::display(std::ostream &os) const {
  os << "StepComputerForce <" << getName() << ">:" << std::endl;
}

void StepComputerForce::commandLine(const std::string &cmdLine,
                                    std::istringstream &cmdArgs,
                                    std::ostream &os) {
  if (cmdLine == "help") {
    os << "NextStep: " << std::endl
       << " - setObserver" << std::endl
       << " - thisIsZero {record|disp}" << std::endl
       << std::endl;
  } else if (cmdLine == "thisIsZero") {
    std::string arg;
    cmdArgs >> arg;
    if (arg == "disp") {
      os << "zero = " << waMref0;
    } else if (arg == "record") {
      thisIsZero();
    }
  } else if (cmdLine == "setObserver") {
    std::string name = "stepobs";
    cmdArgs >> std::ws;
    if (cmdArgs.good()) {
      cmdArgs >> name;
    }
    Entity *entity = &(PoolStorage::getInstance()->getEntity(name));
    twoHandObserver = dynamic_cast<StepObserver *>(entity);
  } else {
  }
}

}  // namespace sot
}  // namespace dynamicgraph


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2			* Copyright Projet JRL-Japan, 2007
3			*+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
4			*
5			* File: StepComputer.cpp
6			* Project: SOT
7			* Author: Paul Evrard, Nicolas Mansard
8			*
9			* Version control
10			* ===============
11			*
12			* $Id$
13			*
14			* Description
15			* ============
16			*
17			*
18			* ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++*/
19
20			#include <time.h>
21
22			#include <cmath>
23			#ifndef WIN32
24			#include <sys/time.h>
25
26			#include <iostream>
27			#else
28			#include <Winsock2.h>
29
30			#include <jrl/mal/boost.hh>
31			#include <sot/core/utils-windows.hh>
32			#endif /WIN32/
33
34			#include <dynamic-graph/factory.h>
35			#include <dynamic-graph/pool.h>
36			#include <sot/pattern-generator/exception-pg.h>
37			#include <sot/pattern-generator/step-checker.h>
38			#include <sot/pattern-generator/step-computer-force.h>
39			#include <sot/pattern-generator/step-queue.h>
40
41			#include <sot/core/debug.hh>
42			#include <sot/core/macros-signal.hh>
43			#include <sot/core/matrix-geometry.hh>
44
45			namespace dynamicgraph {
46			namespace sot {
47
48			DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(StepComputerForce, "StepComputerForce");
49
50			StepComputerForce::StepComputerForce(const std::string &name)
51			: Entity(name),
52			waistMlhandSIN(
53			NULL, "StepComputerForce(" + name + ")::input(vector)::waistMlhand")
54
55			,
56			waistMrhandSIN(
57			NULL, "StepComputerForce(" + name + ")::input(vector)::waistMrhand"),
58			referencePositionWaistSIN(
59			NULL, "StepComputerForce(" + name + ")::input(vector)::posrefwaist")
60
61			,
62			stiffnessSIN(NULL,
63			"StepComputerForce(" + name + ")::input(vector)::stiffness")
64
65			,
66			velocitySIN(NULL,
67			"StepComputerForce(" + name + ")::input(vector)::velocity")
68
69			,
70			contactFootSIN(
71			NULL, "StepComputerForce(" + name + ")::input(uint)::contactfoot")
72
73			,
74			displacementSOUT(
75			boost::bind(&StepComputerForce::computeDisplacement, this, _1, _2),
76			referencePositionWaistSIN,
77			"StepComputerForce(" + name + ")::output(vector)::displacement")
78
79			,
80			forceSOUT(boost::bind(&StepComputerForce::computeForce, this, _1, _2),
81			displacementSOUT,
82			"StepComputerForce(" + name + ")::output(vector)::force")
83
84			,
85			forceLhandSOUT(
86			boost::bind(&StepComputerForce::computeForceL, this, _1, _2),
87			waistMlhandSIN << referencePositionWaistSIN << forceSOUT,
88			"StepComputerForce(" + name + ")::output(vector)::forceL")
89
90			,
91			forceRhandSOUT(
92			boost::bind(&StepComputerForce::computeForceR, this, _1, _2),
93			waistMrhandSIN << referencePositionWaistSIN << forceSOUT,
94			"StepComputerForce(" + name + ")::output(vector)::forceR"),
95			waMref0(),
96			twoHandObserver(0x0),
97			checker(),
98			logChanges("/tmp/stepcomp_changes.dat"),
99			logPreview("/tmp/stepcomp_preview.dat") {
100			sotDEBUGIN(5);
101
102			signalRegistration(referencePositionWaistSIN
103			<< contactFootSIN << waistMlhandSIN << waistMrhandSIN
104			<< stiffnessSIN << velocitySIN << displacementSOUT
105			<< forceSOUT << forceLhandSOUT << forceRhandSOUT);
106
107			sotDEBUGOUT(5);
108			}
109
110			void StepComputerForce::nextStep(StepQueue &queue, int timeCurr) {
111			// Introduce new step at the end of the preview window.
112			if (queue.getLastStep().contact == CONTACT_LEFT_FOOT) {
113			queue.pushStep(0., -queue.getZeroStepPosition(), 0.);
114			logPreview << timeCurr << " " << 0 << " " << -queue.getZeroStepPosition()
115			<< " " << 0 << std::endl;
116			} else {
117			queue.pushStep(0., queue.getZeroStepPosition(), 0.);
118			logPreview << timeCurr << " " << 0 << " " << queue.getZeroStepPosition()
119			<< " " << 0 << std::endl;
120			}
121			}
122
123			Vector &StepComputerForce::computeDisplacement(Vector &res, int timeCurr) {
124			if (!twoHandObserver) {
125			std::cerr << "Observer not set" << std::endl;
126			res.resize(3);
127			res.setZero();
128			return res;
129			}
130
131			// transformation from ref0 to ref.
132
133			const MatrixHomogeneous &waMref = referencePositionWaistSIN.access(timeCurr);
134			MatrixHomogeneous ref0Mwa;
135			ref0Mwa = waMref0.inverse();
136			MatrixHomogeneous ref0Mref;
137			ref0Mref = ref0Mwa * waMref;
138
139			// extract the translation part and express it in the waist frame.
140
141			Vector t_ref0(3);
142			t_ref0 = ref0Mref.translation();
143			MatrixRotation waRref0;
144			waRref0 = waMref0.linear();
145			Vector t_wa = waRref0 * t_ref0;
146
147			// compute the rotation that transforms ref0 into ref,
148			// express it in the waist frame. Then get the associated
149			// yaw (rot around z).
150
151			MatrixRotation ref0Rwa;
152			ref0Rwa = waRref0.transpose();
153			MatrixRotation ref0Rref;
154			ref0Rref = ref0Mref.linear();
155			MatrixRotation tmp;
156			tmp = ref0Rref * ref0Rwa;
157			MatrixRotation Rref;
158			Rref = waRref0 * tmp;
159			VectorRollPitchYaw rpy;
160			rpy = (Rref.eulerAngles(2, 1, 0)).reverse();
161
162			// store the result.
163
164			res.resize(3);
165			res = t_wa;
166			res(2) = rpy(2);
167
168			return res;
169			}
170
171			Vector &StepComputerForce::computeForce(Vector &res, int timeCurr) {
172			const Vector &dx = displacementSOUT.access(timeCurr);
173			const Vector &K = stiffnessSIN.access(timeCurr);
174
175			if ((dx.size() != 3) \|\| (K.size() != 3) \|\| (dx.size() != K.size())) {
176			res.resize(3);
177			res.setZero();
178			} else {
179			res = K * dx;
180			}
181
182			return res;
183			}
184
185			Vector &StepComputerForce::computeHandForce(Vector &res,
186			const MatrixHomogeneous &waMh,
187			const MatrixHomogeneous &waMref,
188			const Vector &F) {
189			if (F.size() != 3) {
190			res.resize(6);
191			res.fill(0.);
192			return res;
193			}
194
195			Vector pref(3);
196			pref = waMref.translation();
197			Vector ph(3);
198			ph = waMh.translation();
199
200			Eigen::Vector3d OA;
201			OA(0) = ph(0) - pref(0);
202			OA(1) = ph(1) - pref(1);
203			OA(2) = 0;
204
205			Eigen::Vector3d tau;
206			tau.setZero();
207			tau(2) = -F(2);
208
209			Eigen::Vector3d tauOA;
210			tauOA = tau.cross(OA);
211			double ntauOA = tauOA.norm();
212			double nOA = OA.norm();
213			double L = 2 * ntauOA * nOA;
214
215			if (L < 1e-12) {
216			tauOA.fill(0);
217			} else {
218			tauOA = tauOA * (1. / L);
219			}
220
221			Vector tmp(6);
222			tmp.setZero();
223			tmp(0) = tauOA(0) - F(0);
224			tmp(1) = tauOA(1) - F(1);
225
226			MatrixHomogeneous H;
227			H = waMh.inverse();
228			for (int i = 0; i < 3; ++i) {
229			H(i, 3) = 0;
230			}
231			MatrixTwist V;
232			buildFrom(H, V);
233			res = V * tmp;
234
235			return res;
236			}
237
238			Vector &StepComputerForce::computeForceL(Vector &res, int timeCurr) {
239			const MatrixHomogeneous &waMlh = waistMlhandSIN.access(timeCurr);
240			const MatrixHomogeneous &waMref = referencePositionWaistSIN.access(timeCurr);
241			const Vector &F = forceSOUT.access(timeCurr);
242
243			return computeHandForce(res, waMlh, waMref, F);
244			}
245
246			Vector &StepComputerForce::computeForceR(Vector &res, int timeCurr) {
247			const MatrixHomogeneous &waMrh = waistMrhandSIN.access(timeCurr);
248			const MatrixHomogeneous &waMref = referencePositionWaistSIN.access(timeCurr);
249			const Vector &F = forceSOUT.access(timeCurr);
250
251			return computeHandForce(res, waMrh, waMref, F);
252			}
253
254			void StepComputerForce::changeFirstStep(StepQueue &queue, int timeCurr) {
255			const Vector &v = velocitySIN.access(timeCurr);
256			unsigned sfoot = contactFootSIN.access(timeCurr);
257
258			double y_default = 0;
259			if (sfoot != 1) { // --- left foot support ---
260			y_default = -0.19;
261			} else { // -- right foot support ---
262			y_default = 0.19;
263			}
264
265			// The clipping function expects the x-y coordinates of the
266			// destination fly foot in the support foot frame.
267
268			double x = v(0), y = v(1);
269			double theta = v(2) * 180 / 3.14159265;
270
271			if (std::abs(x) < 0.03) {
272			x = 0;
273			}
274			if (std::abs(y) < 0.03) {
275			y = 0;
276			}
277			if (std::abs(theta) < 2) {
278			theta = 0;
279			}
280
281			y += y_default;
282
283			const double THETA_MAX = 9.;
284			if (theta < -THETA_MAX) {
285			theta = -THETA_MAX;
286			}
287			if (theta > THETA_MAX) {
288			theta = THETA_MAX;
289			}
290
291			double nx = 0, ny = 0;
292			if (sfoot != 1) { // left foot support phase
293			if (y > 0) {
294			y = -0.001;
295			}
296			} else {
297			if (y < 0) {
298			y = 0.001;
299			}
300			}
301
302			checker.clipStep(x, y, nx, ny);
303
304			// Log x-y values before and after clipping
305
306			logChanges << timeCurr << " " << x << " " << y << " " << nx << " " << ny
307			<< " ";
308
309			// The coordinates must be expressed in the destination foot frame.
310			// See the technical report of Olivier Stasse for more details,
311			// on top of page 79.
312
313			double theta_rad = 3.14159265 * theta / 180.;
314			double ctheta = cos(theta_rad);
315			double stheta = sin(theta_rad);
316
317			x = nx * ctheta + ny * stheta;
318			y = -nx * stheta + ny * ctheta;
319
320			queue.changeFirstStep(x, y, theta);
321
322			// Log the step
323
324			logChanges << x << " " << y << " " << theta << std::endl;
325			}
326
327			void StepComputerForce::thisIsZero() {
328			sotDEBUGIN(15);
329
330			waMref0 = referencePositionWaistSIN.accessCopy();
331
332			sotDEBUGOUT(15);
333			}
334
335			void StepComputerForce::display(std::ostream &os) const {
336			os << "StepComputerForce <" << getName() << ">:" << std::endl;
337			}
338
339			void StepComputerForce::commandLine(const std::string &cmdLine,
340			std::istringstream &cmdArgs,
341			std::ostream &os) {
342			if (cmdLine == "help") {
343			os << "NextStep: " << std::endl
344			<< " - setObserver" << std::endl
345			<< " - thisIsZero {record\|disp}" << std::endl
346			<< std::endl;
347			} else if (cmdLine == "thisIsZero") {
348			std::string arg;
349			cmdArgs >> arg;
350			if (arg == "disp") {
351			os << "zero = " << waMref0;
352			} else if (arg == "record") {
353			thisIsZero();
354			}
355			} else if (cmdLine == "setObserver") {
356			std::string name = "stepobs";
357			cmdArgs >> std::ws;
358			if (cmdArgs.good()) {
359			cmdArgs >> name;
360			}
361			Entity *entity = &(PoolStorage::getInstance()->getEntity(name));
362			twoHandObserver = dynamic_cast<StepObserver *>(entity);
363			} else {
364			}
365			}
366
367			} // namespace sot
368			} // namespace dynamicgraph