diff --git a/lwrserv/SvrData.cpp b/lwrserv/SvrData.cpp index fbb1009..8dbd30c 100644 --- a/lwrserv/SvrData.cpp +++ b/lwrserv/SvrData.cpp @@ -9,17 +9,18 @@ SvrData* SvrData::single = 0; SvrData::SvrData(void) { - velocity = 20; - accelaration = 10; - robot.joints = LBR_MNJ; + // these are the default values for the robot double maxVelJnt[] = {110.0,110.0,128.0,128.0,204.0,184.0,184.0}; double maxAccJnt[] = {1.0,1.0,1.0,1.0,2.0,1.0,1.0}; double maxTrqJnt[] = {176.0,176.0,100.0,100.0,100.0,38.0,38.0}; double maxRangeJnt[] = {170.0,120.0,170.0,120.0,170.0,120.0,170.0}; + robot.currentVelocity = 20; + robot.currentAcceleration = 10; + robot.joints = LBR_MNJ; - for (int i = 0; i < robot.joints; ++i) + for (unsigned int i = 0; i < robot.joints; ++i) { robot.max.velocity[i] = maxVelJnt[i]; robot.max.accelaration[i] = maxAccJnt[i]; @@ -71,6 +72,15 @@ int SvrData::getMessuredJointPos(float* data, size_t size) return 0; } +Vec SvrData::getMessuredJointPos() +{ + Vec buf ; + pthread_mutex_lock(&dataLock); + buf = messured.jointPos; + pthread_mutex_unlock(&dataLock); + return buf; +} + int SvrData::getMessuredCartPos (float* data, size_t size) { if (data == NULL) @@ -84,6 +94,15 @@ int SvrData::getMessuredCartPos (float* data, size_t size) return 0 ; } +Mat SvrData::getMessuredCartPos() +{ + Mat buf ; + pthread_mutex_lock(&dataLock); + buf = messured.cartPos; + pthread_mutex_unlock(&dataLock); + return buf; +} + int SvrData::getMessuredJacobian(float* data, size_t size) { if (data == NULL) @@ -123,7 +142,14 @@ int SvrData::getCommandedJointPos(float* data, size_t size) return 0; } - +Vec SvrData::getCommandedJointPos() +{ + Vec buff; + pthread_mutex_lock(&dataLock); + buff = commanded.jointPos; + pthread_mutex_unlock(&dataLock); + return buff; +} int SvrData::getCommandedCartPos (float* data, size_t size) { if (data == NULL) @@ -137,6 +163,14 @@ int SvrData::getCommandedCartPos (float* data, size_t size) return 0; } +Mat SvrData::getCommandedCartPos () +{ + Mat buff; + pthread_mutex_lock(&dataLock); + buff = commanded.cartPos; + pthread_mutex_unlock(&dataLock); + return buff; +} float SvrData::getMaxTorque(unsigned int pos) { @@ -194,6 +228,37 @@ Vec SvrData::getMaxAcceleration() pthread_mutex_unlock(&dataLock); return buff; } +float SvrData::getMaxVelocity(unsigned int pos) +{ + float retval = -1.0f; + if (pos >= robot.joints) + return retval; + + pthread_mutex_lock(&dataLock); + retval = robot.max.velocity[pos-1]; + pthread_mutex_unlock(&dataLock); + return retval; +} +int SvrData::getMaxVelocity(float* data, size_t size) +{ + if (data == NULL) + return -EINVAL; + if (size != sizeof(float)* robot.joints) + return -EINVAL; + + pthread_mutex_lock(&dataLock); + memcpy(data,robot.max.velocity,size); + pthread_mutex_unlock(&dataLock); + return 0; +} +Vec SvrData::getMaxVelocity() +{ + Vec buff; + pthread_mutex_lock(&dataLock); + buff = robot.max.velocity; + pthread_mutex_unlock(&dataLock); + return buff; +} float SvrData::getMaxRange(unsigned int pos) { @@ -219,7 +284,51 @@ int SvrData::getMaxRange(float* data, size_t size) pthread_mutex_unlock(&dataLock); return 0; } +Vec SvrData::getMaxRange() +{ + Vec retval ; + pthread_mutex_lock(&dataLock); + retval = robot.max.range; + pthread_mutex_unlock(&dataLock); + return 0; +} + +int SvrData::setRobotVelocity(float newVelocity) +{ + unsigned int retval = 0; + pthread_mutex_lock(&dataLock); + robot.currentVelocity = newVelocity; + pthread_mutex_unlock(&dataLock); + return retval; +} +float SvrData::getRobotVelocity() +{ + float retval = 0; + + pthread_mutex_lock(&dataLock); + retval = robot.currentVelocity; + pthread_mutex_unlock(&dataLock); + return retval; +} +int SvrData::setRobotAcceleration(float newAcceleration) +{ + unsigned int retval = 0; + + pthread_mutex_lock(&dataLock); + robot.currentAcceleration = newAcceleration; + pthread_mutex_unlock(&dataLock); + return retval; +} +float SvrData::getRobotAcceleration() +{ + float retval = 0; + + pthread_mutex_lock(&dataLock); + retval = robot.currentAcceleration; + pthread_mutex_unlock(&dataLock); + return retval; +} unsigned int SvrData::getJoints() { unsigned int retval = 0; diff --git a/lwrserv/include/SvrData.h b/lwrserv/include/SvrData.h index f1c9f91..40115e1 100644 --- a/lwrserv/include/SvrData.h +++ b/lwrserv/include/SvrData.h @@ -25,9 +25,6 @@ private: float pose[3][4]; //TODO what is this for } commanded; - float velocity; - float accelaration; - struct { struct { float velocity[LBR_MNJ]; @@ -36,6 +33,8 @@ private: float range[LBR_MNJ]; } max; unsigned int joints; + float currentVelocity; + float currentAcceleration; } robot; pthread_mutex_t dataLock; @@ -54,12 +53,21 @@ public: void unlock(); int getMessuredJointPos(float* data, size_t size); + Vec getMessuredJointPos(); + int getMessuredCartPos (float* data, size_t size); + Mat getMessuredCartPos(); + int getMessuredJacobian(float* data, size_t size); + int getMessuredForceTorque(float* data, size_t size); + Vec getMessuredForceTorque(); int getCommandedJointPos(float* data, size_t size); + Vec getCommandedJointPos(); + int getCommandedCartPos (float* data, size_t size); + Mat getCommandedCartPos(); float getMaxTorque(unsigned int pos); int getMaxTorque(float* data, size_t size); @@ -77,12 +85,12 @@ public: int getMaxRange(float* data, size_t size); Vec getMaxRange(); - unsigned int getJoints(); + float getRobotVelocity(); + int setRobotVelocity(float newVelocity); - float getTrajectoryVelocity(); - int setTrajectoryVelocity(float); + float getRobotAcceleration(); + int setRobotAcceleration(float newVelocity); - float getTrajectoryAccelaration(); - float setTrajectoryAccelaration(float); + unsigned int getJoints(); }; #endif