Functions | |
bool | CLMBase::checkJointSpeed (std::vector< int > lastsolution, std::vector< int > solution, double time) |
CLMBase::CLMBase () | |
bool | CLMBase::getActivatePositionController () |
Check if the position controller will be activated after the linear movement. More... | |
double | CLMBase::getMaximumLinearVelocity () const |
int | CLMBase::getSpeed (int distance, int acceleration, int time) |
JointSpeedException::JointSpeedException () throw () | |
void | CLMBase::moveRobotLinearTo (double x, double y, double z, double phi, double theta, double psi, bool waitUntilReached=true, int waitTimeout=TM_ENDLESS) |
void | CLMBase::moveRobotLinearTo (std::vector< double > coordinates, bool waitUntilReached=true, int waitTimeout=TM_ENDLESS) |
void | CLMBase::moveRobotTo (double x, double y, double z, double phi, double theta, double psi, bool waitUntilReached=true, int waitTimeout=TM_ENDLESS) |
void | CLMBase::moveRobotTo (std::vector< double > coordinates, bool waitUntilReached=true, int waitTimeout=TM_ENDLESS) |
void | CLMBase::movLM (double X, double Y, double Z, double Al, double Be, double Ga, bool exactflag, double vmax, bool wait=true, int tolerance=100, long timeout=TM_ENDLESS) |
void | CLMBase::movLM2P (double X1, double Y1, double Z1, double Al1, double Be1, double Ga1, double X2, double Y2, double Z2, double Al2, double Be2, double Ga2, bool exactflag, double vmax, bool wait=true, int tolerance=100, long timeout=TM_ENDLESS) |
void | CLMBase::movP2P (double X1, double Y1, double Z1, double Ph1, double Th1, double Ps1, double X2, double Y2, double Z2, double Ph2, double Th2, double Ps2, bool exactflag, double vmax, bool wait=true, long timeout=TM_ENDLESS) |
double | CLMBase::relPosition (double reltime, double distance, double acc, double dec, double vmax) |
void | CLMBase::setActivatePositionController (bool activate) |
void | CLMBase::setMaximumLinearVelocity (double maximumVelocity) |
void | CLMBase::splineCoefficients (int steps, double *timearray, double *encoderarray, double *arr_p1, double *arr_p2, double *arr_p3, double *arr_p4) |
double | CLMBase::totalTime (double distance, double acc, double dec, double vmax) |
WaitParameterException::WaitParameterException () throw () | |
Variables | |
bool | CLMBase::_activatePositionController |
double | CLMBase::_maximumVelocity |
|
private |
Checks if the joint speeds are below speed limit.
Maximum joint speed is 180enc / 10ms.
lastsolution | encoder values of last point |
solution | encoder values of current point |
time | time difference between the points in s |
Definition at line 407 of file lmBase.cpp.
bool CLMBase::getActivatePositionController | ( | ) |
Check if the position controller will be activated after the linear movement.
Definition at line 914 of file lmBase.cpp.
double CLMBase::getMaximumLinearVelocity | ( | ) | const |
Definition at line 907 of file lmBase.cpp.
|
private |
Calculates speed from distance, acceleration and time for the movement.
distance | absolute (positive) distance of the movement in encoder |
acceleration | acceleration and deceleration in enc / s^2 |
time | time that can be used for the movement in s |
Definition at line 423 of file lmBase.cpp.
|
inline |
void CLMBase::moveRobotLinearTo | ( | double | x, |
double | y, | ||
double | z, | ||
double | phi, | ||
double | theta, | ||
double | psi, | ||
bool | waitUntilReached = true , |
||
int | waitTimeout = TM_ENDLESS |
||
) |
waitUntilReached | has to be true with new implementation of movLM2P |
Definition at line 869 of file lmBase.cpp.
void CLMBase::moveRobotLinearTo | ( | std::vector< double > | coordinates, |
bool | waitUntilReached = true , |
||
int | waitTimeout = TM_ENDLESS |
||
) |
This method does the same as the one above and is mainly provided for convenience
Definition at line 876 of file lmBase.cpp.
void CLMBase::moveRobotTo | ( | double | x, |
double | y, | ||
double | z, | ||
double | phi, | ||
double | theta, | ||
double | psi, | ||
bool | waitUntilReached = true , |
||
int | waitTimeout = TM_ENDLESS |
||
) |
Moves to robot to given cartesian coordinates and euler-angles.
Definition at line 880 of file lmBase.cpp.
void CLMBase::moveRobotTo | ( | std::vector< double > | coordinates, |
bool | waitUntilReached = true , |
||
int | waitTimeout = TM_ENDLESS |
||
) |
This method does the same as the one above and is mainly provided for convenience
Definition at line 895 of file lmBase.cpp.
void CLMBase::movLM | ( | double | X, |
double | Y, | ||
double | Z, | ||
double | Al, | ||
double | Be, | ||
double | Ga, | ||
bool | exactflag, | ||
double | vmax, | ||
bool | wait = true , |
||
int | tolerance = 100 , |
||
long | timeout = TM_ENDLESS |
||
) |
wait | has to be true with new implementation of movLM2P |
Definition at line 853 of file lmBase.cpp.
void CLMBase::movLM2P | ( | double | X1, |
double | Y1, | ||
double | Z1, | ||
double | Al1, | ||
double | Be1, | ||
double | Ga1, | ||
double | X2, | ||
double | Y2, | ||
double | Z2, | ||
double | Al2, | ||
double | Be2, | ||
double | Ga2, | ||
bool | exactflag, | ||
double | vmax, | ||
bool | wait = true , |
||
int | tolerance = 100 , |
||
long | timeout = TM_ENDLESS |
||
) |
Move linear from point to point using multiple splines.
X1,Y1,Z1,Ph1,Th1,Ps1 | X, Y, Z, Phi, Theta, Psi of actual position |
X2,Y2,Z2,Ph2,Th2,Ps2 | X, Y, Z, Phi, Theta, Psi of target position |
exactflag | activate the position controller after the movement |
vmax | maximum velocity of the movement in mm/s |
wait | wait for end of movement |
tolerance | tolerance for all motor encoders |
timeout | timeout for linear movement in ms |
NoSolutionException | if no solution found for IK |
JointSpeedException | if joint speed too high |
WaitParameterException | if wait set to false |
Definition at line 32 of file lmBase.cpp.
void CLMBase::movP2P | ( | double | X1, |
double | Y1, | ||
double | Z1, | ||
double | Ph1, | ||
double | Th1, | ||
double | Ps1, | ||
double | X2, | ||
double | Y2, | ||
double | Z2, | ||
double | Ph2, | ||
double | Th2, | ||
double | Ps2, | ||
bool | exactflag, | ||
double | vmax, | ||
bool | wait = true , |
||
long | timeout = TM_ENDLESS |
||
) |
Move point to point using splines.
X1,Y1,Z1,Ph1,Th1,Ps1 | X, Y, Z, Phi, Theta, Psi of actual position |
X2,Y2,Z2,Ph2,Th2,Ps2 | X, Y, Z, Phi, Theta, Psi of target position |
exactflag | activate the position controller after the movement |
vmax | maximum velocity for motors |
wait | wait for end of movement |
tolerance | tolerance for all motor encoders |
timeout | timeout for movement in ms |
NoSolutionException | if no solution found for IK |
JointSpeedException | if joint speed too high |
WaitParameterException | if wait set to false |
Definition at line 457 of file lmBase.cpp.
|
private |
Calculates the relative position reached after the relative time given.
reltime | relative time (fraction of totaltime) |
distance | distance of the movement in mm |
acc | acceleration at the beginning in mm/s^2 |
dec | deceleration at the end in mm/s^2 |
vmax | maximum velocity of the movement in mm/s |
Definition at line 284 of file lmBase.cpp.
void CLMBase::setActivatePositionController | ( | bool | activate | ) |
Re-Activate the position controller after the linear movement.
Definition at line 911 of file lmBase.cpp.
void CLMBase::setMaximumLinearVelocity | ( | double | maximumVelocity | ) |
Definition at line 900 of file lmBase.cpp.
|
private |
Calculates the spline coefficient and stores them in arr_p1 - arr_p4.
Boundary conditions are that f_1'=0 and f_n'=0 (zero velocity at beginning and end of the movement) and f_i''=P_(i+1)''.
steps | number of splines to calculate |
timearray | times of the points (length = steps + 1) |
encoderarray | encoder values of the points (length = steps + 1) |
arr_p1 | to return parameters 1 (length = steps) |
arr_p2 | to return parameters 2 (length = steps) |
arr_p3 | to return parameters 3 (length = steps) |
arr_p4 | to return parameters 4 (length = steps) |
Definition at line 316 of file lmBase.cpp.
|
private |
Calculates time needed for movement over a distance.
distance | distance of the movement in mm |
acc | acceleration at the beginning in mm/s^2 |
dec | deceleration at the end in mm/s^2 |
vmax | maximum velocity of the movement in mm/s |
Definition at line 267 of file lmBase.cpp.
|
inline |