Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/catkin/dg_control/dg_tools/src/control/calibrator.cpp	Lines:	30	67	44.8 %
Date:	2020-04-15 11:50:02	Branches:	58	144	40.3 %


/*
 * Copyright 2010,
 * Steve Heim
 *
 * CNRS/AIST
 *
 * This file is part of sot-core.
 * sot-core is free software: you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation, either version 3 of
 * the License, or (at your option) any later version.
 * sot-core is distributed in the hope that it will be
 * useful, but WITHOUT ANY WARRANTY; without even the implied warranty
 * of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.  You should
 * have received a copy of the GNU Lesser General Public License along
 * with sot-core.  If not, see <http://www.gnu.org/licenses/>.
 */

/* SOT */
#include "dg_tools/control/calibrator.hpp"

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>

using namespace dynamicgraph::sot;
using namespace dynamicgraph;

DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Calibrator, "Calibrator");

const double Calibrator::
TIME_STEP_DEFAULT = .001;

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */


#define __SOT_Calibrator_INIT \

Calibrator::Calibrator( const std::string & name )
 :Entity(name)
 ,positionSIN(NULL,"Calibrator("+name+")::input(vector)::raw_position")
 ,velocitySIN(NULL,"Calibrator("+name+")::input(vector)::velocity")
 ,calibration_torqueSIN(NULL,
              "Calibrator("+name+")::input(vector)::calibration_torque")
 ,hardstop2zeroSIN(NULL,"Calibrator("+name+")::input(vector)::hardstop2zero")
 ,positionSOUT( boost::bind(&Calibrator::compute_position,this,_1,_2),
         positionSIN << hardstop2zeroSIN,
        "Calibrator("+name+")::output(vector)::calibrated_position" )
 ,controlSOUT( boost::bind(&Calibrator::calibrate,this,_1,_2),
  positionSIN << velocitySIN << calibration_torqueSIN,
 "Calibrator("+name+")::output(vector)::control" )
 ,calibrated_flagSOUT( boost::bind(&Calibrator::is_calibrated,this,_1,_2),
  internal_signal_refresher_,"Calibrator("+name+
  ")::output(vector)::calibrated_flag" )
 ,internal_signal_refresher_("Calibrator("+name+")::intern(dummy)::refresher")
 ,init_flag(1)
 ,calibrated_flag_(0) // now a signal_OUT
 ,threshold_time(1000) // threshold_time used to ramp up
 ,threshold_velocity(0.001)
{
  internal_signal_refresher_.setDependencyType(
    dynamicgraph::TimeDependency<int>::ALWAYS_READY);

  Entity::signalRegistration( positionSIN << velocitySIN << hardstop2zeroSIN <<
                      calibration_torqueSIN << positionSOUT << controlSOUT
                      << calibrated_flagSOUT);
}

// TODO: setters for thresholds

void Calibrator::display( std::ostream& os ) const
{
  os << "Calibrator "<<getName();
  try{
    os <<"control = "<<controlSOUT;
  }
  catch (ExceptionSignal e) {}
  os <<" ("<<TimeStep<<") ";
}

/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* --------------------------------------------------------------------- */

double& Calibrator::setsize(int dimension)
{
  _dimension = dimension;
  return _dimension;
}

dynamicgraph::Vector&
    Calibrator::calibrate( dynamicgraph::Vector &torque, int t )
{
  sotDEBUGIN(15);

  const dynamicgraph::Vector& position = positionSIN(t);
  const dynamicgraph::Vector& velocity = velocitySIN(t);
  const dynamicgraph::Vector& calibration_torque = calibration_torqueSIN(t);

  num_joints = position.size();
  torque.resize(num_joints); torque.setZero();
  if(init_flag){
    t_start = t;
    error.resize(num_joints); error.setZero();
    calibrated.resize(num_joints); calibrated.setZero();
    start2hardstop.resize(num_joints); start2hardstop.setZero();

    init_flag = 0;
  }

  // ramp to calibration torque in threshold_time, then hold

  for(int idx = 0; idx < num_joints; idx++){
    // turn motors off after calibration is finished
    if(calibrated[idx]){
        torque[idx]=0;
    } else {
      if((t-t_start)<=threshold_time){ //ramp
        torque[idx] = calibration_torque[idx]/threshold_time*(t-t_start);
      } else { //and hold
        torque[idx] = calibration_torque[idx];
      }
    }
    // check if we've saturated on error. If yes, save position and flip flag
    if((t-t_start)>threshold_time && // finished ramp
        abs(velocity[idx])<threshold_velocity && // joint has stopped
        !calibrated[idx]) // hasn't already been calibrated
      {
      start2hardstop[idx] = position[idx];
      calibrated[idx] = true;
    }
  }

  // if all joints are calibrated, yay.
  if(calibrated.sum() == num_joints && !calibrated_flag_){
    calibrated_flag_ = 1;
  }
  sotDEBUGOUT(15);
  return torque;
}

int &Calibrator::
is_calibrated( int &calibrated_flag, int t)
{
  // calibrated_flag.resize(1,1);// = calibrated_flag_;
  calibrated_flag = calibrated_flag_;
  return calibrated_flag;
}

dynamicgraph::Vector &Calibrator::
compute_position(dynamicgraph::Vector &calibratedPosition, int t)
{
    sotDEBUGIN(15);

    const dynamicgraph::Vector &position = positionSIN(t);
    const dynamicgraph::Vector &hardstop2zero = hardstop2zeroSIN(t);

    calibratedPosition.resize(position.size());
    if(start2hardstop.size() == 0){ // calibration has not yet happened!
      start2hardstop.resize(position.size());
      start2hardstop.setZero();
    }

    calibratedPosition.array() = position.array() - start2hardstop.array()
                                 + hardstop2zero.array();

    sotDEBUGOUT(15);
    return calibratedPosition;
}


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Copyright 2010,
3			* Steve Heim
4			*
5			* CNRS/AIST
6			*
7			* This file is part of sot-core.
8			* sot-core is free software: you can redistribute it and/or
9			* modify it under the terms of the GNU Lesser General Public License
10			* as published by the Free Software Foundation, either version 3 of
11			* the License, or (at your option) any later version.
12			* sot-core is distributed in the hope that it will be
13			* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
14			* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15			* GNU Lesser General Public License for more details. You should
16			* have received a copy of the GNU Lesser General Public License along
17			* with sot-core. If not, see <http://www.gnu.org/licenses/>.
18			*/
19
20			/* SOT */
21			#include "dg_tools/control/calibrator.hpp"
22
23			/* --------------------------------------------------------------------- */
24			/* --------------------------------------------------------------------- */
25			/* --------------------------------------------------------------------- */
26			#include <sot/core/debug.hh>
27			#include <dynamic-graph/factory.h>
28
29			using namespace dynamicgraph::sot;
30			using namespace dynamicgraph;
31
32	✓✗✓✗ ✓✗✓✗	4	DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(Calibrator, "Calibrator");
33
34			const double Calibrator::
35			TIME_STEP_DEFAULT = .001;
36
37			/* --------------------------------------------------------------------- */
38			/* --------------------------------------------------------------------- */
39			/* --------------------------------------------------------------------- */
40
41
42			#define __SOT_Calibrator_INIT \
43
44		3	Calibrator::Calibrator( const std::string & name )
45			:Entity(name)
46	✓✗✓✗	6	,positionSIN(NULL,"Calibrator("+name+")::input(vector)::raw_position")
47	✓✗✓✗	6	,velocitySIN(NULL,"Calibrator("+name+")::input(vector)::velocity")
48			,calibration_torqueSIN(NULL,
49	✓✗✓✗	6	"Calibrator("+name+")::input(vector)::calibration_torque")
50	✓✗✓✗	6	,hardstop2zeroSIN(NULL,"Calibrator("+name+")::input(vector)::hardstop2zero")
51			,positionSOUT( boost::bind(&Calibrator::compute_position,this,_1,_2),
52	✓✗	6	positionSIN << hardstop2zeroSIN,
53	✓✗✓✗	6	"Calibrator("+name+")::output(vector)::calibrated_position" )
54			,controlSOUT( boost::bind(&Calibrator::calibrate,this,_1,_2),
55	✓✗✓✗	9	positionSIN << velocitySIN << calibration_torqueSIN,
56	✓✗✓✗	6	"Calibrator("+name+")::output(vector)::control" )
57			,calibrated_flagSOUT( boost::bind(&Calibrator::is_calibrated,this,_1,_2),
58	✓✗✓✗	6	internal_signal_refresher_,"Calibrator("+name+
59			")::output(vector)::calibrated_flag" )
60	✓✗✓✗	6	,internal_signal_refresher_("Calibrator("+name+")::intern(dummy)::refresher")
61			,init_flag(1)
62			,calibrated_flag_(0) // now a signal_OUT
63			,threshold_time(1000) // threshold_time used to ramp up
64	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	33	,threshold_velocity(0.001)
65			{
66		3	internal_signal_refresher_.setDependencyType(
67		3	dynamicgraph::TimeDependency<int>::ALWAYS_READY);
68
69	✓✗✓✗	9	Entity::signalRegistration( positionSIN << velocitySIN << hardstop2zeroSIN <<
70	✓✗✓✗ ✓✗	3	calibration_torqueSIN << positionSOUT << controlSOUT
71	✓✗✓✗	6	<< calibrated_flagSOUT);
72		3	}
73
74			// TODO: setters for thresholds
75
76			void Calibrator::display( std::ostream& os ) const
77			{
78			os << "Calibrator "<<getName();
79			try{
80			os <<"control = "<<controlSOUT;
81			}
82			catch (ExceptionSignal e) {}
83			os <<" ("<<TimeStep<<") ";
84			}
85
86			/* --------------------------------------------------------------------- */
87			/* --------------------------------------------------------------------- */
88			/* --------------------------------------------------------------------- */
89
90			double& Calibrator::setsize(int dimension)
91			{
92			_dimension = dimension;
93			return _dimension;
94			}
95
96			dynamicgraph::Vector&
97			Calibrator::calibrate( dynamicgraph::Vector &torque, int t )
98			{
99			sotDEBUGIN(15);
100
101			const dynamicgraph::Vector& position = positionSIN(t);
102			const dynamicgraph::Vector& velocity = velocitySIN(t);
103			const dynamicgraph::Vector& calibration_torque = calibration_torqueSIN(t);
104
105			num_joints = position.size();
106			torque.resize(num_joints); torque.setZero();
107			if(init_flag){
108			t_start = t;
109			error.resize(num_joints); error.setZero();
110			calibrated.resize(num_joints); calibrated.setZero();
111			start2hardstop.resize(num_joints); start2hardstop.setZero();
112
113			init_flag = 0;
114			}
115
116			// ramp to calibration torque in threshold_time, then hold
117
118			for(int idx = 0; idx < num_joints; idx++){
119			// turn motors off after calibration is finished
120			if(calibrated[idx]){
121			torque[idx]=0;
122			} else {
123			if((t-t_start)<=threshold_time){ //ramp
124			torque[idx] = calibration_torque[idx]/threshold_time*(t-t_start);
125			} else { //and hold
126			torque[idx] = calibration_torque[idx];
127			}
128			}
129			// check if we've saturated on error. If yes, save position and flip flag
130			if((t-t_start)>threshold_time && // finished ramp
131			abs(velocity[idx])<threshold_velocity && // joint has stopped
132			!calibrated[idx]) // hasn't already been calibrated
133			{
134			start2hardstop[idx] = position[idx];
135			calibrated[idx] = true;
136			}
137			}
138
139			// if all joints are calibrated, yay.
140			if(calibrated.sum() == num_joints && !calibrated_flag_){
141			calibrated_flag_ = 1;
142			}
143			sotDEBUGOUT(15);
144			return torque;
145			}
146
147			int &Calibrator::
148			is_calibrated( int &calibrated_flag, int t)
149			{
150			// calibrated_flag.resize(1,1);// = calibrated_flag_;
151			calibrated_flag = calibrated_flag_;
152			return calibrated_flag;
153			}
154
155		1	dynamicgraph::Vector &Calibrator::
156			compute_position(dynamicgraph::Vector &calibratedPosition, int t)
157			{
158			sotDEBUGIN(15);
159
160		1	const dynamicgraph::Vector &position = positionSIN(t);
161		1	const dynamicgraph::Vector &hardstop2zero = hardstop2zeroSIN(t);
162
163		1	calibratedPosition.resize(position.size());
164	✓✗	1	if(start2hardstop.size() == 0){ // calibration has not yet happened!
165		1	start2hardstop.resize(position.size());
166		1	start2hardstop.setZero();
167			}
168
169	✓✗✓✗ ✓✗✓✗	4	calibratedPosition.array() = position.array() - start2hardstop.array()
170	✓✗✓✗	5	+ hardstop2zero.array();
171
172			sotDEBUGOUT(15);
173		1	return calibratedPosition;
174	✓✗✓✗	6	}