Hekateros  3.4.3
RoadNarrows Robotics Robot Arm Project
Public Member Functions | Protected Attributes | List of all members
hekateros::HekCalib Class Referenceabstract

Hekateros robotic manipulator calibration abstract base class. More...

#include <hekCalib.h>

Inheritance diagram for hekateros::HekCalib:
hekateros::HekCalibStretch

Public Member Functions

 HekCalib (HekRobot &robot)
 
virtual ~HekCalib ()
 
virtual int calibrate ()=0
 Calibrate the Hekateros's robotic arm. More...
 
virtual double moveWait (const std::string &strJointName, const double fJointGoalPos, const double fJointGoalVel)
 Move joint to position. More...
 
virtual double moveToTorqueLimit (const std::string &strJointName, const double fJointGoalPos, const double fJointGoalVel)
 Move joint until torque limit is reached. More...
 
virtual double moveToLight (const std::string &strJointName, const double fJointGoalPos, const double fJointGoalVel, byte_t byMask)
 Move joint until unoccluded optical limit position is detected. More...
 
virtual double moveToDark (const std::string &strJointName, const double fJointGoalPos, const double fJointGoalVel, byte_t byMask)
 Move joint until occluded optical limit position is detected. More...
 

Protected Attributes

HekRobot & m_robot
 robot instance
 

Detailed Description

Hekateros robotic manipulator calibration abstract base class.

Parameters
robotInstance of Hekateros robot.

Definition at line 73 of file hekCalib.h.

Constructor & Destructor Documentation

hekateros::HekCalib::HekCalib ( HekRobot &  robot)
inline

Default initialization constructor.

Parameters
robotInstance of Hekateros robot.

Definition at line 82 of file hekCalib.h.

82  : m_robot(robot)
83  {
84  }
hekateros::HekCalib::m_robot
HekRobot & m_robot
robot instance
Definition: hekCalib.h:165
virtual hekateros::HekCalib::~HekCalib ( )
inlinevirtual

Destructor.

Definition at line 89 of file hekCalib.h.

References calibrate(), moveToDark(), moveToLight(), moveToTorqueLimit(), and moveWait().

90  {
91  }

Member Function Documentation

virtual int hekateros::HekCalib::calibrate ( )
pure virtual

Calibrate the Hekateros's robotic arm.

Returns
On success, HEK_OK is returned.
On error, the appropriate < 0 negated Hekateros Error Code is returned.

Implemented in hekateros::HekCalibStretch.

Referenced by ~HekCalib().

double HekCalib::moveToDark ( const std::string &  strJointName,
const double  fJointGoalPos,
const double  fJointGoalVel,
byte_t  byMask 
)
virtual

Move joint until occluded optical limit position is detected.

This call blocks until move is complete.

Parameters
strJointNameJoint name.
fJointGoalPosJoint goal position (radians).
fJointGoalVelJoint goal velocity (radians/second).
byMaskMask of limits to check.
Returns
New joint position (radians) at first dark.

Definition at line 241 of file hekCalib.cxx.

References hekateros::getDarkOpticalLimits(), and hekateros::radToDeg().

Referenced by ~HekCalib().

245 {
246  static const int TuneMaxSecs = 30; // max of 30 seconds to complete move
247  static const int TuneWaituSec = 200000; // usec sleep between tests
248 
249  int nMaxIters = TuneMaxSecs * MILLION / TuneWaituSec; // max iterations
250 
251  double fJointCurPos, fJointCurVel;
252  byte_t byLimits;
253 
254  LOGDIAG3("%s(): Move %s to goalpos=%.2lf at goalvel=%.3lf", LOGFUNCNAME,
255  strJointName.c_str(), radToDeg(fJointGoalPos), radToDeg(fJointGoalVel));
256 
257  // start move
258  m_robot.m_pKin->move(strJointName, fJointGoalPos, fJointGoalVel);
259 
260  // wait until move is completed or maximum seconds reached.
261  for(int i=0; i<nMaxIters; ++i)
262  {
263  // physically blocked
264  if( m_robot.m_pKin->hasOverTorqueCondition(strJointName) )
265  {
266  LOGDIAG3("%s(): %s in over torque condition.", LOGFUNCNAME,
267  strJointName.c_str());
268  break;
269  }
270 
271  // joint kinematics stopped
272  else if( m_robot.m_pKin->isStopped(strJointName) )
273  {
274  LOGDIAG3("%s(): %s is stopped.", LOGFUNCNAME, strJointName.c_str());
275  break;
276  }
277 
278  // optical limits
279  byLimits = m_robot.m_monitor.getJointLimitBits();
280 
281  // dark
282  if( getDarkOpticalLimits(byLimits, byMask) != 0 )
283  {
284  LOGDIAG3("%s(): %s reached the dark.", LOGFUNCNAME,
285  strJointName.c_str());
286  break;
287  }
288 
289  m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);
290 
291  LOGDIAG3("%s(): %s at curpos=%.2lf, curvel=%.3lf", LOGFUNCNAME,
292  strJointName.c_str(), radToDeg(fJointCurPos), radToDeg(fJointCurVel));
293 
294  // sleep (and a pthread cancelation point)
295  usleep(TuneWaituSec);
296  }
297 
298  m_robot.m_pKin->stop(strJointName);
299 
300  usleep(TuneWaituSec);
301 
302  byLimits = m_robot.m_monitor.getJointLimitBits();
303 
304  if( getDarkOpticalLimits(byLimits, byMask) == 0 )
305  {
306  LOGWARN("%s(): %s stopped before finding the dark.", LOGFUNCNAME,
307  strJointName.c_str());
308  }
309 
310  m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);
311 
312  LOGDIAG3("%s(): Joint %s move ended at to curpos=%.2lf", LOGFUNCNAME,
313  strJointName.c_str(), radToDeg(fJointCurPos));
314 
315  return fJointCurPos;
316 }
hekateros::HekKinematics::stop
virtual int stop()
Stop kinematics chain at the current position.
Definition: hekKin.cxx:399
hekateros::HekMonitor::getJointLimitBits
byte_t getJointLimitBits()
Get current limit switch bit values.
Definition: hekMonitor.h:413
hekateros::HekKinematics::getJointCurPosVel
virtual void getJointCurPosVel(const std::string &strJointName, double &fJointCurPos, double &fJointCurVel)
Get the current instantaneous joint position and velocity.
Definition: hekKin.cxx:242
hekateros::HekCalib::m_robot
HekRobot & m_robot
robot instance
Definition: hekCalib.h:165
hekateros::HekKinematics::isStopped
virtual bool isStopped(const std::string &strJointName)
Test if joint is moving.
Definition: hekKin.cxx:308
hekateros::HekRobot::m_pKin
HekKinematics * m_pKin
dynamics and kinematics
Definition: hekRobot.h:703
hekateros::radToDeg
double radToDeg(double r)
Convert radians to degrees.
Definition: hekUtils.h:137
hekateros::getDarkOpticalLimits
static byte_t getDarkOpticalLimits(byte_t byBits, byte_t byMask)
Test if any of the optical limits have been triggered (occluded).
Definition: hekOptical.h:194
hekateros::HekKinematics::hasOverTorqueCondition
virtual bool hasOverTorqueCondition(const std::string &strJointName)
Test if joint is in an over torque condition.
Definition: hekKin.cxx:322
hekateros::HekRobot::m_monitor
HekMonitor m_monitor
power, health, and safety monitor
Definition: hekRobot.h:709
hekateros::HekKinematics::move
virtual int move(HekJointTrajectoryPoint &trajectoryPoint)
Move kinematic chain through a trajectory point.
Definition: hekKin.cxx:500
double HekCalib::moveToLight ( const std::string &  strJointName,
const double  fJointGoalPos,
const double  fJointGoalVel,
byte_t  byMask 
)
virtual

Move joint until unoccluded optical limit position is detected.

This call blocks until move is complete.

Parameters
strJointNameJoint name.
fJointGoalPosJoint goal position (radians).
fJointGoalVelJoint goal velocity (radians/second).
byMaskMask of limits to check.
Returns
New joint position (radians) at first light.

Definition at line 164 of file hekCalib.cxx.

References hekateros::getLitOpticalLimits(), and hekateros::radToDeg().

Referenced by ~HekCalib().

168 {
169  static const int TuneMaxSecs = 30; // max of 30 seconds to complete move
170  static const int TuneWaituSec = 200000; // usec sleep between tests
171 
172  int nMaxIters = TuneMaxSecs * MILLION / TuneWaituSec; // max iterations
173 
174  double fJointCurPos, fJointCurVel;
175  byte_t byLimits;
176 
177  LOGDIAG3("%s(): Move %s to goalpos=%.2lf at goalvel=%.3lf", LOGFUNCNAME,
178  strJointName.c_str(), radToDeg(fJointGoalPos), radToDeg(fJointGoalVel));
179 
180  // start move
181  m_robot.m_pKin->move(strJointName, fJointGoalPos, fJointGoalVel);
182 
183  // wait until move is completed or maximum seconds reached.
184  for(int i=0; i<nMaxIters; ++i)
185  {
186  // physically blocked
187  if( m_robot.m_pKin->hasOverTorqueCondition(strJointName) )
188  {
189  LOGDIAG3("%s(): %s in over torque condition.", LOGFUNCNAME,
190  strJointName.c_str());
191  break;
192  }
193 
194  // joint kinematics stopped
195  else if( m_robot.m_pKin->isStopped(strJointName) )
196  {
197  LOGDIAG3("%s(): %s is stopped.", LOGFUNCNAME, strJointName.c_str());
198  break;
199  }
200 
201  // optical limits
202  byLimits = m_robot.m_monitor.getJointLimitBits();
203 
204  // light
205  if( getLitOpticalLimits(byLimits, byMask) != 0 )
206  {
207  LOGDIAG3("%s(): %s reached the light.", LOGFUNCNAME,
208  strJointName.c_str());
209  break;
210  }
211 
212  m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);
213 
214  LOGDIAG3("%s(): %s at curpos=%.2lf, curvel=%.3lf", LOGFUNCNAME,
215  strJointName.c_str(), radToDeg(fJointCurPos), radToDeg(fJointCurVel));
216 
217  // sleep (and a pthread cancelation point)
218  usleep(TuneWaituSec);
219  }
220 
221  m_robot.m_pKin->stop(strJointName);
222 
223  usleep(TuneWaituSec);
224 
225  byLimits = m_robot.m_monitor.getJointLimitBits();
226 
227  if( getLitOpticalLimits(byLimits, byMask) == 0 )
228  {
229  LOGWARN("%s(): %s stopped before finding the light.", LOGFUNCNAME,
230  strJointName.c_str());
231  }
232 
233  m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);
234 
235  LOGDIAG3("%s(): Joint %s move ended at to curpos=%.2lf", LOGFUNCNAME,
236  strJointName.c_str(), radToDeg(fJointCurPos));
237 
238  return fJointCurPos;
239 }
hekateros::HekKinematics::stop
virtual int stop()
Stop kinematics chain at the current position.
Definition: hekKin.cxx:399
hekateros::HekMonitor::getJointLimitBits
byte_t getJointLimitBits()
Get current limit switch bit values.
Definition: hekMonitor.h:413
hekateros::HekKinematics::getJointCurPosVel
virtual void getJointCurPosVel(const std::string &strJointName, double &fJointCurPos, double &fJointCurVel)
Get the current instantaneous joint position and velocity.
Definition: hekKin.cxx:242
hekateros::HekCalib::m_robot
HekRobot & m_robot
robot instance
Definition: hekCalib.h:165
hekateros::HekKinematics::isStopped
virtual bool isStopped(const std::string &strJointName)
Test if joint is moving.
Definition: hekKin.cxx:308
hekateros::HekRobot::m_pKin
HekKinematics * m_pKin
dynamics and kinematics
Definition: hekRobot.h:703
hekateros::radToDeg
double radToDeg(double r)
Convert radians to degrees.
Definition: hekUtils.h:137
hekateros::getLitOpticalLimits
static byte_t getLitOpticalLimits(byte_t byBits, byte_t byMask)
Test if any of the optical limits are lit (not occluded).
Definition: hekOptical.h:209
hekateros::HekKinematics::hasOverTorqueCondition
virtual bool hasOverTorqueCondition(const std::string &strJointName)
Test if joint is in an over torque condition.
Definition: hekKin.cxx:322
hekateros::HekRobot::m_monitor
HekMonitor m_monitor
power, health, and safety monitor
Definition: hekRobot.h:709
hekateros::HekKinematics::move
virtual int move(HekJointTrajectoryPoint &trajectoryPoint)
Move kinematic chain through a trajectory point.
Definition: hekKin.cxx:500
double HekCalib::moveToTorqueLimit ( const std::string &  strJointName,
const double  fJointGoalPos,
const double  fJointGoalVel 
)
virtual

Move joint until torque limit is reached.

This call blocks until move is complete.

Parameters
strJointNameJoint name.
fJointGoalPosJoint goal position (radians).
fJointGoalVelJoint goal velocity (radians/second).
Returns
New joint position (radians) at over torque condition.

Definition at line 110 of file hekCalib.cxx.

References hekateros::radToDeg().

Referenced by ~HekCalib().

113 {
114  static const int TuneMaxSecs = 30; // max of 30 seconds to complete move
115  static const int TuneWaituSec = 300000; // usec sleep between tests
116 
117  int nMaxIters = TuneMaxSecs * MILLION / TuneWaituSec; // max iterations
118 
119  double fJointCurPos, fJointCurVel;
120 
121  LOGDIAG3("%s(): Move joint %s to goalpos=%.2lf at goalvel=%.3lf", LOGFUNCNAME,
122  strJointName.c_str(), radToDeg(fJointGoalPos), radToDeg(fJointGoalVel));
123 
124  // start move
125  m_robot.m_pKin->move(strJointName, fJointGoalPos, fJointGoalVel);
126 
127  //
128  // Wait until move is completed, interrupted, or the maximum seconds reached.
129  //
130  for(int i=0; i<nMaxIters; ++i)
131  {
132  // physically blocked
133  if( m_robot.m_pKin->hasOverTorqueCondition(strJointName) )
134  {
135  LOGDIAG3("%s(): %s in over torque condition.", LOGFUNCNAME,
136  strJointName.c_str());
137  break;
138  }
139 
140  // joint kinematics stopped
141  else if( m_robot.m_pKin->isStopped(strJointName) )
142  {
143  LOGDIAG3("%s(): %s is stopped.", LOGFUNCNAME, strJointName.c_str());
144  break;
145  }
146 
147  m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);
148 
149  LOGDIAG3("%s(): %s at curpos=%.2lf, curvel=%.3lf", LOGFUNCNAME,
150  strJointName.c_str(), radToDeg(fJointCurPos), radToDeg(fJointCurVel));
151 
152  // sleep (and a pthread cancelation point)
153  usleep(TuneWaituSec);
154  }
155 
156  m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);
157 
158  LOGDIAG3("%s(): Joint %s move ended at to curpos=%.2lf", LOGFUNCNAME,
159  strJointName.c_str(), radToDeg(fJointCurPos));
160 
161  return fJointCurPos;
162 }
hekateros::HekKinematics::getJointCurPosVel
virtual void getJointCurPosVel(const std::string &strJointName, double &fJointCurPos, double &fJointCurVel)
Get the current instantaneous joint position and velocity.
Definition: hekKin.cxx:242
hekateros::HekCalib::m_robot
HekRobot & m_robot
robot instance
Definition: hekCalib.h:165
hekateros::HekKinematics::isStopped
virtual bool isStopped(const std::string &strJointName)
Test if joint is moving.
Definition: hekKin.cxx:308
hekateros::HekRobot::m_pKin
HekKinematics * m_pKin
dynamics and kinematics
Definition: hekRobot.h:703
hekateros::radToDeg
double radToDeg(double r)
Convert radians to degrees.
Definition: hekUtils.h:137
hekateros::HekKinematics::hasOverTorqueCondition
virtual bool hasOverTorqueCondition(const std::string &strJointName)
Test if joint is in an over torque condition.
Definition: hekKin.cxx:322
hekateros::HekKinematics::move
virtual int move(HekJointTrajectoryPoint &trajectoryPoint)
Move kinematic chain through a trajectory point.
Definition: hekKin.cxx:500
double HekCalib::moveWait ( const std::string &  strJointName,
const double  fJointGoalPos,
const double  fJointGoalVel 
)
virtual

Move joint to position.

This call blocks until move is complete.

Parameters
strJointNameJoint name.
fJointGoalPosJoint goal position (radians).
fJointGoalVelJoint goal velocity (radians/second).
Returns
New joint position (radians).

Definition at line 65 of file hekCalib.cxx.

References hekateros::radToDeg().

Referenced by ~HekCalib().

68 {
69  static const int TuneMaxSecs = 30; // max of 30 seconds to complete move
70  static const int TuneWaituSec = 300000; // usec sleep between tests
71 
72  int nMaxIters = TuneMaxSecs * MILLION / TuneWaituSec; // max iterations
73 
74  double fJointCurPos, fJointCurVel;
75 
76  LOGDIAG3("%s(): Move joint %s to goalpos=%.2lf at goalvel=%.3lf", LOGFUNCNAME,
77  strJointName.c_str(), radToDeg(fJointGoalPos), radToDeg(fJointGoalVel));
78 
79  // start move
80  m_robot.m_pKin->move(strJointName, fJointGoalPos, fJointGoalVel);
81 
82  //
83  // Wait until move is completed, interrupted, or the maximum seconds reached.
84  //
85  for(int i=0; i<nMaxIters; ++i)
86  {
87  if( m_robot.m_pKin->isStopped(strJointName) )
88  {
89  LOGDIAG3("%s(): %s is stopped.", LOGFUNCNAME, strJointName.c_str());
90  break;
91  }
92 
93  m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);
94 
95  LOGDIAG3("%s(): %s at curpos=%.2lf, curvel=%.3lf", LOGFUNCNAME,
96  strJointName.c_str(), radToDeg(fJointCurPos), radToDeg(fJointCurVel));
97 
98  // sleep (and a pthread cancelation point)
99  usleep(TuneWaituSec);
100  }
101 
102  m_robot.m_pKin->getJointCurPosVel(strJointName, fJointCurPos, fJointCurVel);
103 
104  LOGDIAG3("%s(): Joint %s move ended at to curpos=%.2lf", LOGFUNCNAME,
105  strJointName.c_str(), radToDeg(fJointCurPos));
106 
107  return fJointCurPos;
108 }
hekateros::HekKinematics::getJointCurPosVel
virtual void getJointCurPosVel(const std::string &strJointName, double &fJointCurPos, double &fJointCurVel)
Get the current instantaneous joint position and velocity.
Definition: hekKin.cxx:242
hekateros::HekCalib::m_robot
HekRobot & m_robot
robot instance
Definition: hekCalib.h:165
hekateros::HekKinematics::isStopped
virtual bool isStopped(const std::string &strJointName)
Test if joint is moving.
Definition: hekKin.cxx:308
hekateros::HekRobot::m_pKin
HekKinematics * m_pKin
dynamics and kinematics
Definition: hekRobot.h:703
hekateros::radToDeg
double radToDeg(double r)
Convert radians to degrees.
Definition: hekUtils.h:137
hekateros::HekKinematics::move
virtual int move(HekJointTrajectoryPoint &trajectoryPoint)
Move kinematic chain through a trajectory point.
Definition: hekKin.cxx:500
The documentation for this class was generated from the following files: