14#include <robottestingframework/TestAssert.h>
56 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(
dd->
isValid(),
"Unable to open device driver");
57 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(
dd->
view(
ienc),
"Unable to open encoders interface");
58 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(
dd->
view(
ipos),
"Unable to open position interface");
59 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(
dd->
view(
icmd),
"Unable to open control mode interface");
60 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(
dd->
view(
iimd),
"Unable to open interaction mode interface");
61 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(
dd->
view(
ilim),
"Unable to open limits interface");
67 for(
size_t i = 0; i < n_joints; i++) {
68 if(jointsList[i] == j) {
72 return jointsList.size()+1;
78 max_lims.resize(n_joints);
79 min_lims.resize(n_joints);
80 for (
size_t i = 0; i < n_joints; i++) {
81 ilim->getPosLimits((
int)jointsList[i], &min_lims[i], &max_lims[i]);
91 mPriv->
init(polydriver);
101 for (
size_t i = 0; i < mPriv->
n_joints; i++) {
117 mPriv->tolerance = tolerance;
123 for (
size_t i = 0; i < mPriv->jointsList.size(); i++) {
135 for (
size_t i = 0; i < mPriv->n_joints; i++) {
136 mPriv->icmd->getControlMode ((
int)mPriv->jointsList[i], cmode);
137 mPriv->iimd->getInteractionMode((
int)mPriv->jointsList[i], imode);
142 if (ok == mPriv->n_joints) {
146 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR(
"Unable to set control mode/interaction mode");
159 mPriv->timeout = timeout;
166 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE((speedlist.
size() != mPriv->jointsList.size()),
"Speed list has a different size of joint list");
167 mPriv->speed = speedlist;
168 for (
size_t i = 0; i < mPriv->n_joints; i++) {
169 mPriv->ipos->setTrajSpeed((
int)mPriv->jointsList[i], mPriv->speed[i]);
176 size_t i = mPriv->getIndexOfJoint(j);
177 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(i < mPriv->n_joints,
"Cannot move a joint not in list.");
179 mPriv->ipos->positionMove((
int)mPriv->jointsList[i], pos);
185 mPriv->ienc->getEncoder((
int)mPriv->jointsList[i], &tmp);
186 if (fabs(tmp-pos)<mPriv->tolerance) {
198 if(reached_pos !=
nullptr) {
207 for (
unsigned int i=0; i<mPriv->n_joints; i++) {
208 mPriv->ipos->positionMove((
int)mPriv->jointsList[i], positions[i]);
216 size_t in_position = 0;
217 for (
size_t i = 0; i < mPriv->n_joints; i++) {
218 mPriv->ienc->getEncoder((
int)mPriv->jointsList[i], &tmp[i]);
219 if (fabs(tmp[i] - positions[i]) < mPriv->tolerance) {
223 if (in_position == mPriv->n_joints) {
234 if(reached_pos !=
nullptr) {
235 reached_pos->
resize(mPriv->n_joints);
245 size_t i = mPriv->getIndexOfJoint(j);
247 ROBOTTESTINGFRAMEWORK_ASSERT_ERROR_IF_FALSE(i < mPriv->n_joints,
"Cannot move a joint not in list.");
250 mPriv->ienc->getEncoder((
int)mPriv->jointsList[i], &enc);
251 if (fabs(enc-mPriv->max_lims[i]) < mPriv->tolerance ||
252 fabs(enc-mPriv->min_lims[i]) < mPriv->tolerance ) {
define control board standard interfaces
contains the definition of a Vector type
bool view(T *&x)
Get an interface to the device driver.
Interface for control devices, commands to get/set position and velocity limits.
Interface for setting control mode in control board.
Control board, encoder interface.
Interface settings the way the robot interacts with the environment: basic interaction types are Stif...
Interface for a generic control board device implementing position control.
virtual yarp::dev::ReturnValue setTrajSpeed(int j, double sp)=0
Set reference speed for a joint, this is the speed used during the interpolation of the trajectory.
A container for a device driver.
bool isValid() const
Check if device is valid.
yarp::dev::IInteractionMode * iimd
yarp::dev::IControlMode * icmd
yarp::sig::Vector jointsList
yarp::dev::PolyDriver * dd
yarp::sig::Vector max_lims
size_t getIndexOfJoint(int j)
yarp::dev::IPositionControl * ipos
yarp::dev::IEncoders * ienc
void init(yarp::dev::PolyDriver *polydriver)
yarp::sig::Vector encoders
yarp::sig::Vector min_lims
yarp::dev::IControlLimits * ilim
virtual ~jointsPosMotion()
void setTolerance(double tolerance)
Sets tolerance used to check if a joint is in position.
void setTimeout(double timeout)
Sets timeout.
bool goToSingle(int j, double pos, double *reached_pos=nullptr)
Moves joint j in position pos and checks if joint reaches target within tolerance range in maximun ti...
bool checkJointLimitsReached(int j)
Checks if joint j has reached its limit within tollerance range.
bool setAndCheckPosControlMode()
Sets all joints in position control mode and checks if all are in position control mode waiting timeo...
bool goTo(yarp::sig::Vector positions, yarp::sig::Vector *reached_pos=nullptr)
Moves joints in corresponding positions specified by positions and checks if all joints reach its tar...
void setSpeed(yarp::sig::Vector &speedlist)
Sets speed of each joint used in position control.
jointsPosMotion(yarp::dev::PolyDriver *polydriver, yarp::sig::Vector &jlist)
Creates an object for control joints specified in @jlist.
void zero()
Set to zero the elements of the vector.
size_t size() const
Returns the number of elements in the vector.
void resize(size_t size)
Resize the vector.
double now()
Return the current time in seconds, relative to an arbitrary starting point.
void delay(double seconds)
Wait for a certain number of seconds.