|
|
|
@ -10,8 +10,8 @@ |
|
|
|
|
|
|
|
#include "Mat.h"
|
|
|
|
#include "Vec.h"
|
|
|
|
#include "Robot.h"
|
|
|
|
#include "lwr4.h"
|
|
|
|
#include "Robot.h"
|
|
|
|
|
|
|
|
bool SvrData::instanceFlag = false; |
|
|
|
SvrData* SvrData::single = 0; |
|
|
|
@ -34,7 +34,7 @@ SvrData::SvrData(void) |
|
|
|
robot.currentAccelerationPercentage = 10; |
|
|
|
|
|
|
|
// use the lwr4 robot by default
|
|
|
|
robot.robotClass = new LWR4(); |
|
|
|
robot.robotClass = (class Robot*)new LWR4(); |
|
|
|
|
|
|
|
// use 5ms interval for each step
|
|
|
|
trajectory.sampleTimeMs = 5.0f; |
|
|
|
@ -117,9 +117,9 @@ void SvrData::updateMeasurementData( |
|
|
|
{ |
|
|
|
pthread_mutex_lock(&dataLock); |
|
|
|
|
|
|
|
this->messured.jointPos = newJointPos; |
|
|
|
this->messured.cartPos = newCartPos; |
|
|
|
this->messured.forceAndTorque = newForceAndTorque; |
|
|
|
this->measured.jointPos = newJointPos; |
|
|
|
this->measured.cartPos = newCartPos; |
|
|
|
this->measured.forceAndTorque = newForceAndTorque; |
|
|
|
|
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
} |
|
|
|
@ -155,47 +155,6 @@ void SvrData::updateMeasurement() |
|
|
|
updateMeasurementData(newJointPos,newCartPos,newForceAndTorque); |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function returns the current measurement of the joints |
|
|
|
* within the database. |
|
|
|
* This value will be updated if updateMeasurement was called |
|
|
|
* which is done by the robot server thread. |
|
|
|
* |
|
|
|
* @see updateMeasurement |
|
|
|
* @see threadRobotMovement |
|
|
|
*/ |
|
|
|
Robot::VecJoint SvrData::getMeasuredJointPos() |
|
|
|
{ |
|
|
|
Robot::VecJoint buf; |
|
|
|
|
|
|
|
pthread_mutex_lock(&dataLock); |
|
|
|
buf = messured.jointPos; |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
|
|
|
|
return buf; |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function returns the current measurement homogeneous |
|
|
|
* cartesian positions within the database. |
|
|
|
* |
|
|
|
* This value will be updated if updateMeasurement was called |
|
|
|
* which is done by the robot server thread. |
|
|
|
* |
|
|
|
* @see updateMeasurement |
|
|
|
* @see threadRobotMovement |
|
|
|
*/ |
|
|
|
MatCarthesian SvrData::getMessuredCartPos() |
|
|
|
{ |
|
|
|
MatCarthesian buf; |
|
|
|
|
|
|
|
pthread_mutex_lock(&dataLock); |
|
|
|
buf = messured.cartPos; |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
|
|
|
|
return buf; |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function returns the current measurement force and |
|
|
|
* torque within the database. |
|
|
|
@ -206,12 +165,12 @@ MatCarthesian SvrData::getMessuredCartPos() |
|
|
|
* @see updateMeasurement |
|
|
|
* @see threadRobotMovement |
|
|
|
*/ |
|
|
|
VecTorque SvrData::getMessuredForceTorque() |
|
|
|
VecTorque SvrData::getMeasuredForceTorque() |
|
|
|
{ |
|
|
|
VecTorque buf; |
|
|
|
|
|
|
|
pthread_mutex_lock(&dataLock); |
|
|
|
buf = messured.forceAndTorque; |
|
|
|
buf = measured.forceAndTorque; |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
|
|
|
|
return buf; |
|
|
|
@ -231,15 +190,15 @@ VecTorque SvrData::getMessuredForceTorque() |
|
|
|
* @see updateMeasurement |
|
|
|
* @see threadRobotMovement |
|
|
|
*/ |
|
|
|
int SvrData::getMessuredJacobian(float* data, size_t size) |
|
|
|
int SvrData::getMeasuredJacobian(float* data, size_t size) |
|
|
|
{ |
|
|
|
if (data == NULL) |
|
|
|
return -EINVAL; |
|
|
|
if (size != sizeof(messured.jacobian)) |
|
|
|
if (size != sizeof(measured.jacobian)) |
|
|
|
return -EINVAL; |
|
|
|
|
|
|
|
pthread_mutex_lock(&dataLock); |
|
|
|
memcpy(data,messured.jacobian,size); |
|
|
|
memcpy(data,measured.jacobian,size); |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
|
|
|
|
return 0; |
|
|
|
@ -395,58 +354,73 @@ Robot::VecJoint SvrData::getMinRange() |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function returns the current minimum range per joint |
|
|
|
* for the configured Robot. |
|
|
|
* This function checks with the current configured robot |
|
|
|
* the valid range |
|
|
|
* |
|
|
|
* @return Vector containing the minimum angle for the joints [ degree ] |
|
|
|
* @return Vector containing the angle for the joints [ degree ] |
|
|
|
*/ |
|
|
|
bool SvrData::checkJointRange(Robot::VecJoint vectorToCheck) |
|
|
|
{ |
|
|
|
// save temporal the max range for short time lock
|
|
|
|
bool retval = false; |
|
|
|
|
|
|
|
pthread_mutex_lock(&dataLock); |
|
|
|
Vec<float,LBR_MNJ> maxJointRange; |
|
|
|
maxJointRange = robot.max.range; |
|
|
|
retval = this->robot.robotClass->validateJointRange(vectorToCheck); |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
|
|
|
|
// check if the value is in plus or minus range
|
|
|
|
for (int i = 0; i< robot.robotClass->joints; i++) |
|
|
|
{ |
|
|
|
if (abs(vectorToCheck(i)) > abs(maxJointRange(i))) |
|
|
|
{ |
|
|
|
// join angle is too big
|
|
|
|
return false; |
|
|
|
} |
|
|
|
} |
|
|
|
// no join angle is too big
|
|
|
|
return true; |
|
|
|
return retval; |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function sets the current percentage of the |
|
|
|
* maximum velocity for the configured Robot. |
|
|
|
* The percentage has to be in the interval of (0,100] |
|
|
|
* to be valid. |
|
|
|
* |
|
|
|
* @param newVelocityPercentage the new percentage value |
|
|
|
* @return 0 if it was successfully otherwise the negative Error Code |
|
|
|
*/ |
|
|
|
int SvrData::setRobotVelocityPercentage(float newVelocityPercentage) |
|
|
|
{ |
|
|
|
unsigned int retval = 0; |
|
|
|
// percentage is to low
|
|
|
|
if (newVelocityPercentage <= 0.0f) |
|
|
|
return -1; |
|
|
|
return -EINVAL; |
|
|
|
// percentage is to high
|
|
|
|
if (newVelocityPercentage > 100.0f) |
|
|
|
return -2; |
|
|
|
return -EINVAL; |
|
|
|
|
|
|
|
pthread_mutex_lock(&dataLock); |
|
|
|
robot.currentVelocityPercentage = newVelocityPercentage; |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
|
|
|
|
return retval; |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function returns the current velocity of the configured Robot. |
|
|
|
* |
|
|
|
* @return The velocity in [ degree/seconds ] |
|
|
|
*/ |
|
|
|
Robot::VecJoint SvrData::getRobotVelocity() |
|
|
|
{ |
|
|
|
float percent = 0.0f; |
|
|
|
Robot::VecJoint retval(0.0f); |
|
|
|
|
|
|
|
pthread_mutex_lock(&dataLock); |
|
|
|
// calculate the multiplier for the max velocity
|
|
|
|
percent = robot.currentVelocityPercentage / 100.0f; |
|
|
|
retval = robot.robotClass->getJointVelocity() * percent; |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
|
|
|
|
return retval; |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function returns the current percentage of the maximum velocity |
|
|
|
* of the configured robot. |
|
|
|
* |
|
|
|
* @return The percentage within the interval of (0,100] |
|
|
|
*/ |
|
|
|
float SvrData::getRobotVelocityPercentage() |
|
|
|
{ |
|
|
|
float retval = 0; |
|
|
|
@ -457,34 +431,56 @@ float SvrData::getRobotVelocityPercentage() |
|
|
|
|
|
|
|
return retval; |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function sets the current percentage of the maximum acceleration |
|
|
|
* of the configured robot. |
|
|
|
* |
|
|
|
* @param newAccelerationPercentage the new percentage value for the acceleration |
|
|
|
* @return 0 if the new values was set correctly and the negative Error code otherwise |
|
|
|
*/ |
|
|
|
int SvrData::setRobotAccelerationPercentage(float newAccelerationPercentage) |
|
|
|
{ |
|
|
|
int retval = 0; |
|
|
|
// percentage is to low
|
|
|
|
if (newAccelerationPercentage <= 0.0f) |
|
|
|
return -1; |
|
|
|
return -EINVAL; |
|
|
|
// percentage is to high
|
|
|
|
if (newAccelerationPercentage > 100.0f) |
|
|
|
return -2; |
|
|
|
return -EINVAL; |
|
|
|
|
|
|
|
pthread_mutex_lock(&dataLock); |
|
|
|
robot.currentAccelerationPercentage = newAccelerationPercentage; |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
|
|
|
|
return retval; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function returns the current acceleration |
|
|
|
* of the configured robot. |
|
|
|
* |
|
|
|
* @return the current acceleration in [ degree / seconds^2 ] |
|
|
|
*/ |
|
|
|
Robot::VecJoint SvrData::getRobotAcceleration() |
|
|
|
{ |
|
|
|
float percent = 0; |
|
|
|
Robot::VecJoint retval(0.0f); |
|
|
|
|
|
|
|
pthread_mutex_lock(&dataLock); |
|
|
|
// calculate the percentage multiplier
|
|
|
|
percent = robot.currentAccelerationPercentage / 100.0f; |
|
|
|
retval = robot.robotClass->getJointAcceleration() * percent; |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
|
|
|
|
return retval; |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function returns the current percentage of acceleration |
|
|
|
* of the configured robot. |
|
|
|
* |
|
|
|
* @return the current acceleration in [ percent ] |
|
|
|
*/ |
|
|
|
float SvrData::getRobotAccelerationPercentage() |
|
|
|
{ |
|
|
|
float retval = 0; |
|
|
|
@ -495,18 +491,47 @@ float SvrData::getRobotAccelerationPercentage() |
|
|
|
|
|
|
|
return retval; |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function returns the robot class reference |
|
|
|
* @info robot class has only get/calculate function so returning |
|
|
|
* the reference could not do any harm |
|
|
|
* |
|
|
|
* @return the reference to the robot instance |
|
|
|
*/ |
|
|
|
class Robot* SvrData::getRobot() |
|
|
|
{ |
|
|
|
class Robot* retval; |
|
|
|
|
|
|
|
pthread_mutex_lock(&dataLock); |
|
|
|
retval = this->robot.robotClass; |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
|
|
|
|
return retval; |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function returns the joint number of the current robot |
|
|
|
* class |
|
|
|
* |
|
|
|
* @return the number of joints of the current robot |
|
|
|
*/ |
|
|
|
unsigned int SvrData::getJoints() |
|
|
|
{ |
|
|
|
unsigned int retval = 0; |
|
|
|
|
|
|
|
pthread_mutex_lock(&dataLock); |
|
|
|
retval = Robot::joints; |
|
|
|
retval = this->robot.robotClass->joints; |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
|
|
|
|
return retval; |
|
|
|
} |
|
|
|
|
|
|
|
/// Trajectory function
|
|
|
|
/**
|
|
|
|
* This function returns the current configured sample time of the robot |
|
|
|
* |
|
|
|
* @return the time interval of the FRI Interface in [ milliseconds ] |
|
|
|
*/ |
|
|
|
float SvrData::getSampleTimeMs() |
|
|
|
{ |
|
|
|
float retval = 0; |
|
|
|
@ -514,26 +539,43 @@ float SvrData::getSampleTimeMs() |
|
|
|
pthread_mutex_lock(&dataLock); |
|
|
|
retval = trajectory.sampleTimeMs; |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
|
|
|
|
return retval; |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function changes the sample time of the FRI interface. |
|
|
|
* The unit is in [ milliseconds ]. |
|
|
|
* The time interval must be within the interval (0, infinity) |
|
|
|
* |
|
|
|
* @return 0 if the sample interval was set correctly and the negative error code otherwise |
|
|
|
*/ |
|
|
|
int SvrData::setSampleTimeMs(float newSampleTimeMs) |
|
|
|
{ |
|
|
|
int retval = 0; |
|
|
|
|
|
|
|
// sample time can not be negative or 0
|
|
|
|
if (newSampleTimeMs <= 0.0f) |
|
|
|
return -1; |
|
|
|
return -EINVAL; |
|
|
|
|
|
|
|
pthread_mutex_lock(&dataLock); |
|
|
|
trajectory.sampleTimeMs = newSampleTimeMs; |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
return retval; |
|
|
|
|
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function grabs the next trajectory from the list. |
|
|
|
* The time interval must be within the interval (0, infinity) |
|
|
|
* |
|
|
|
* @return a valid Trajectory pointer if the next trajectory could be grabbed. |
|
|
|
* Otherwise the returnvalue will be NULL, which also could be happen if there is no trajectory. |
|
|
|
*/ |
|
|
|
class Trajectory<Robot::joints>* SvrData::popTrajectory() |
|
|
|
{ |
|
|
|
class Trajectory<Robot::joints>* retval = NULL; |
|
|
|
|
|
|
|
pthread_mutex_lock(&dataLock); |
|
|
|
// check if there is a trajectory
|
|
|
|
if (!trajectory.list.empty() ) |
|
|
|
{ |
|
|
|
retval = trajectory.list.front(); |
|
|
|
@ -541,12 +583,21 @@ class Trajectory<Robot::joints>* SvrData::popTrajectory() |
|
|
|
} |
|
|
|
else |
|
|
|
{ |
|
|
|
// no trajectroy left
|
|
|
|
// no trajectory left set the signal that each trajectory was grabbed
|
|
|
|
trajectory.done = true; |
|
|
|
} |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
|
|
|
|
return retval; |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function grabs the next trajectory from the list. |
|
|
|
* The time interval must be within the interval (0, infinity) |
|
|
|
* |
|
|
|
* @return a valid Trajectory pointer if the next trajectory could be grabbed. |
|
|
|
* Otherwise the returnvalue will be NULL, which also could be happen if there is no trajectory. |
|
|
|
*/ |
|
|
|
int SvrData::pushTrajectory(class Trajectory<Robot::joints>* newTrajectory) |
|
|
|
{ |
|
|
|
if (newTrajectory == NULL) |
|
|
|
@ -558,15 +609,37 @@ int SvrData::pushTrajectory(class Trajectory<Robot::joints>* newTrajectory) |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
return 0; |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function grabs the flag if the current trajectory has to be |
|
|
|
* canceled. |
|
|
|
* |
|
|
|
* @return true if the trajectory has to be about |
|
|
|
* Otherwise the returnvalue will be NULL, which also could be happen if there is no trajectory. |
|
|
|
* |
|
|
|
* @see cancelCurrentTrajectory |
|
|
|
*/ |
|
|
|
bool SvrData::getTrajectoryCancel() |
|
|
|
{ |
|
|
|
bool retval = false; |
|
|
|
|
|
|
|
pthread_mutex_lock(&dataLock); |
|
|
|
retval = trajectory.cancel; |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
|
|
|
|
return retval; |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function sets the cancel flag and deletes all trajectories within the queue. |
|
|
|
* The current trajectory will be canceled when the server thread sees the cancel flag. |
|
|
|
* |
|
|
|
* @return true if the trajectory has to be about |
|
|
|
* Otherwise the returnvalue will be NULL, which also could be happen if there is no trajectory. |
|
|
|
* |
|
|
|
* @return true if all trajectory could be canceled and the flag could be set correctly. Otherwise false. |
|
|
|
* @see getTrajectoryCancel |
|
|
|
*/ |
|
|
|
bool SvrData::cancelCurrentTrajectory() |
|
|
|
{ |
|
|
|
pthread_mutex_lock(&dataLock); |
|
|
|
@ -596,8 +669,16 @@ bool SvrData::cancelCurrentTrajectory() |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
boost::this_thread::sleep( boost::posix_time::milliseconds(trajectory.sampleTimeMs) ); |
|
|
|
} |
|
|
|
|
|
|
|
return true; |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function resets the cancel flag. |
|
|
|
* |
|
|
|
* @see getTrajectoryCancel |
|
|
|
* @see getTrajectoryDone |
|
|
|
*/ |
|
|
|
void SvrData::trajectoryCancelDone() |
|
|
|
{ |
|
|
|
pthread_mutex_lock(&dataLock); |
|
|
|
@ -605,6 +686,14 @@ void SvrData::trajectoryCancelDone() |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function returns if the server thread canceled the trajectory correcly. |
|
|
|
* |
|
|
|
* @return true if the trajectory was canceled false otherwise. |
|
|
|
* |
|
|
|
* @see getTrajectoryCancel |
|
|
|
* @see trajectoryCancelDone |
|
|
|
*/ |
|
|
|
bool SvrData::getTrajectoryDone() |
|
|
|
{ |
|
|
|
bool retval = true; |
|
|
|
@ -614,6 +703,14 @@ bool SvrData::getTrajectoryDone() |
|
|
|
return retval; |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function returns the current configured type of trajectory. |
|
|
|
* |
|
|
|
* @return the enumeration id of the current trajectory type |
|
|
|
* |
|
|
|
* @see TrajectoryType |
|
|
|
* @see setTrajectoryType |
|
|
|
*/ |
|
|
|
enum TrajectoryType SvrData::getTrajectoryType() |
|
|
|
{ |
|
|
|
enum TrajectoryType retval = TrajectoryDefault; |
|
|
|
@ -622,6 +719,15 @@ enum TrajectoryType SvrData::getTrajectoryType() |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
return retval; |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function changes the configured type of trajectory. |
|
|
|
* |
|
|
|
* @param newType the new enumeration id of the new type |
|
|
|
* |
|
|
|
* @see TrajectoryType |
|
|
|
* @see getTrajectoryType |
|
|
|
*/ |
|
|
|
void SvrData::setTrajectoryType(enum TrajectoryType newType) |
|
|
|
{ |
|
|
|
pthread_mutex_lock(&dataLock); |
|
|
|
@ -629,6 +735,16 @@ void SvrData::setTrajectoryType(enum TrajectoryType newType) |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function creates an instance of the current configured |
|
|
|
* trajectory from the last commanded position to the parameter |
|
|
|
* provided. |
|
|
|
* |
|
|
|
* @param newJointPos the new commanded position defined by joints |
|
|
|
* |
|
|
|
* @see TrajectoryType |
|
|
|
* @see createTrajectory |
|
|
|
*/ |
|
|
|
class Trajectory<Robot::joints>* SvrData::createTrajectory(Robot::VecJoint newJointPos) |
|
|
|
{ |
|
|
|
//set new target for next trajectory
|
|
|
|
@ -640,45 +756,106 @@ class Trajectory<Robot::joints>* SvrData::createTrajectory(Robot::VecJoint newJo |
|
|
|
Robot::VecJoint maxJointAcceleration = getRobotAcceleration(); |
|
|
|
enum TrajectoryType type = getTrajectoryType(); |
|
|
|
|
|
|
|
//newTrajectory = SvrData::getInstance()->createTrajectory(newJointPos);
|
|
|
|
newTrajectory = Trajectory<Robot::joints>::GetTrajectory(type,sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos); |
|
|
|
if ( this->getTrajectoryPotfieldMode() == false) |
|
|
|
newTrajectory = Trajectory<Robot::joints>::GetTrajectory(type,sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos); |
|
|
|
else |
|
|
|
newTrajectory = new Trajectory<Robot::joints>(sampleTimeMs, maxJointVelocity, maxJointAcceleration, prevJointPos, newJointPos); |
|
|
|
|
|
|
|
return newTrajectory; |
|
|
|
} |
|
|
|
class Trajectory<Robot::joints>* SvrData::createTrajectory(MatCarthesian newJointPos) |
|
|
|
/**
|
|
|
|
* This function creates an instance of the current configured |
|
|
|
* trajectory from the last commanded position to the parameter |
|
|
|
* provided. |
|
|
|
* |
|
|
|
* @param newCartPos the new commanded position defined by the homogeneous cartesian position |
|
|
|
* |
|
|
|
* @see TrajectoryType |
|
|
|
* @see createTrajectory |
|
|
|
*/ |
|
|
|
class Trajectory<Robot::joints>* SvrData::createTrajectory(MatCarthesian newCartPos) |
|
|
|
{ |
|
|
|
(void) newJointPos; |
|
|
|
// unimplemented
|
|
|
|
return NULL; |
|
|
|
// TODO check all combinations and take the one with the minimal joint movement
|
|
|
|
struct Robot::RobotConfig robotconfig; |
|
|
|
robotconfig.elbow = false; |
|
|
|
robotconfig.flip = false; |
|
|
|
robotconfig.j1os = false; |
|
|
|
|
|
|
|
// calculate the joint angle for the position
|
|
|
|
Robot::VecJoint newJointPos = this->robot.robotClass->backwardCalc(newCartPos,robotconfig); |
|
|
|
|
|
|
|
return createTrajectory(newJointPos); |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function returns the current measured position for each |
|
|
|
* joint. |
|
|
|
* These values will be updated by the server via the updateMeasurement |
|
|
|
* function. |
|
|
|
* |
|
|
|
* @return the current joint positions in [ degree ] |
|
|
|
* |
|
|
|
* @see updateMeasurement |
|
|
|
* @see updateMeasurementData |
|
|
|
*/ |
|
|
|
Robot::VecJoint SvrData::getMeasuredJointPos() |
|
|
|
{ |
|
|
|
Robot::VecJoint retval(0.0f); |
|
|
|
|
|
|
|
pthread_mutex_lock(&dataLock); |
|
|
|
retval = messured.jointPos; |
|
|
|
retval = measured.jointPos; |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
|
|
|
|
return retval; |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function returns the measured homogenious cartesian postition. |
|
|
|
* These values will be updated by the server via the updateMeasurement |
|
|
|
* function. |
|
|
|
* |
|
|
|
* @return the homogenious cartesian position |
|
|
|
* |
|
|
|
* @see updateMeasurement |
|
|
|
* @see updateMeasurementData |
|
|
|
*/ |
|
|
|
MatCarthesian SvrData::getMeasuredCartPos() |
|
|
|
{ |
|
|
|
MatCarthesian retval(0.0f,1.0f); |
|
|
|
|
|
|
|
pthread_mutex_lock(&dataLock); |
|
|
|
retval = messured.cartPos; |
|
|
|
retval = measured.cartPos; |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
|
|
|
|
return retval; |
|
|
|
} |
|
|
|
|
|
|
|
/**
|
|
|
|
* This function sets the flag to start the potential field mode. |
|
|
|
* |
|
|
|
* @param newstate the new state of the flag. False to stop and true to start the potential field mode. |
|
|
|
* @see getTrajectoryPotfieldMode |
|
|
|
*/ |
|
|
|
void SvrData::setTrajectoryPotfieldMode(bool newstate) |
|
|
|
{ |
|
|
|
pthread_mutex_lock(&dataLock); |
|
|
|
trajectory.potfieldmode = newstate; |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
} |
|
|
|
/**
|
|
|
|
* This function returns the flag of the potential file mode. |
|
|
|
* |
|
|
|
* @return True if the potential field mode is active and false otherwise. |
|
|
|
* |
|
|
|
* @see setTrajectoryPotfieldMode |
|
|
|
*/ |
|
|
|
bool SvrData::getTrajectoryPotfieldMode() |
|
|
|
{ |
|
|
|
bool retval = false; |
|
|
|
|
|
|
|
pthread_mutex_lock(&dataLock); |
|
|
|
retval = trajectory.potfieldmode; |
|
|
|
pthread_mutex_unlock(&dataLock); |
|
|
|
|
|
|
|
return retval; |
|
|
|
} |
philipp schoenberger
10 years ago