Skip to content

Commit

Permalink
Merge branch 'yarp-deprecations' into develop
Browse files Browse the repository at this point in the history
  • Loading branch information
@PeterBowman
PeterBowman committed May 21, 2017
2 parents 7df420d + 609041c commit 6e0232e
Show file tree
Hide file tree
Showing 46 changed files with 481 additions and 463 deletions.
20 changes: 15 additions & 5 deletions example/python/exampleRemoteControlboard.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,23 +46,33 @@
options.put('local','/exampleRemoteControlboard') # we add info on how we will call ourselves on the YARP network
dd = yarp.PolyDriver(options) # create a YARP multi-use driver with the given options

pos = dd.viewIPositionControl() # make a position controller object we call 'pos'
mode = dd.viewIControlMode() # make a operation mode controller object we call 'mode'

enc = dd.viewIEncoders() # make an encoder controller object we call 'enc'
axes = enc.getAxes() # retrieve number of joints

# use the object to set the device to position mode (as opposed to velocity mode)(note: stops the robot)
for j in range(1,axes):
mode.setPositionMode(j)

pos.setPositionMode() # use the object to set the device to position mode (as opposed to velocity mode)(note: stops the robot)
pos = dd.viewIPositionControl() # make a position controller object we call 'pos'

print 'test positionMove(1,-35) -> moves motor 1 (start count at motor 0) to -35 degrees'
pos.positionMove(1,-35)

print 'test delay(5)'
yarp.Time.delay(5)

enc = dd.viewIEncoders() # make an encoder controller object we call 'enc'
v = yarp.DVector(enc.getAxes()) # create a YARP vector of doubles the size of the number of elements read by enc, call it 'v'
v = yarp.DVector(axes) # create a YARP vector of doubles the size of the number of elements read by enc, call it 'v'
enc.getEncoders(v) # read the encoder values and put them into 'v'
print 'v[1] is: ' + str(v[1]) # print element 1 of 'v', note that motors and encoders start at 0

# use the object to set the device to velocity mode (as opposed to position mode)
for j in range(1,axes):
mode.setPositionMode(j)

vel = dd.viewIVelocityControl() # make a velocity controller object we call 'pos'
vel.setVelocityMode() # use the object to set the device to velocity mode (as opposed to position mode)

print 'test velocityMove(0,10) -> moves motor 0 (start count at motor 0) at 10 degrees per second'
vel.velocityMove(0,10)

Expand Down
2 changes: 1 addition & 1 deletion libraries/BodyYarp/CanBusControlboard/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,7 @@ IF (NOT SKIP_CanBusControlboard)
# Include any directories needed for YARP
include_directories(${YARP_INCLUDE_DIRS} ${ROBOTICSLAB_YARP_DEVICES_INCLUDE_DIRS} ${COLOR_DEBUG_INCLUDE_DIRS} ${CMAKE_CURRENT_SOURCE_DIR})

yarp_add_plugin(CanBusControlboard DeviceDriverImpl.cpp IControlLimits2Impl.cpp IControlMode2.cpp IEncodersImpl.cpp IEncodersTimedImpl.cpp IPositionControl2.cpp IPositionDirectImpl.cpp ITorqueControlImpl.cpp IVelocityControl2.cpp CanBusControlboard.cpp IInteractionMode.cpp ThreadImpl.cpp)
yarp_add_plugin(CanBusControlboard DeviceDriverImpl.cpp IControlLimits2Impl.cpp IControlMode2Impl.cpp IEncodersImpl.cpp IEncodersTimedImpl.cpp IPositionControl2Impl.cpp IPositionDirectImpl.cpp ITorqueControlImpl.cpp IVelocityControl2Impl.cpp CanBusControlboard.cpp IInteractionModeImpl.cpp ThreadImpl.cpp)
TARGET_LINK_LIBRARIES(CanBusControlboard ${YARP_LIBRARIES})

# Exporting dependencies for ROBOTICSLAB_YARP_DEVICESConfig.cmake quite manually for now...
Expand Down
51 changes: 8 additions & 43 deletions libraries/BodyYarp/CanBusControlboard/CanBusControlboard.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -105,7 +105,7 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo
*/
virtual bool getVelLimits(int axis, double *min, double *max);

// --------- IControlMode Declarations. Implementation in IControlModeImpl.cpp ---------
// --------- IControlMode Declarations. Implementation in IControlMode2Impl.cpp ---------

/**
* Set position mode, single axis.
Expand Down Expand Up @@ -164,7 +164,7 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo
*/
virtual bool getControlModes(int *modes);

// --------- IControlMode2 Declarations. Implementation in IControlMode2.cpp ---------
// --------- IControlMode2 Declarations. Implementation in IControlMode2Impl.cpp ---------

/**
* Get the current control mode for a subset of axes.
Expand Down Expand Up @@ -309,7 +309,7 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo
*/
virtual bool getEncoderTimed(int j, double *encs, double *time);

// ------- IPositionControl declarations. Implementation in IPositionControl2.cpp -------
// ------- IPositionControl declarations. Implementation in IPositionControl2Impl.cpp -------

/**
* Get the number of controlled axes. This command asks the number of controlled
Expand All @@ -319,14 +319,6 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo
*/
virtual bool getAxes(int *ax);

/** Set position mode. This command
* is required by control boards implementing different
* control methods (e.g. velocity/torque), in some cases
* it can be left empty.
* return true/false on success/failure
*/
virtual bool setPositionMode();

/** Set new reference point for a single axis.
* @param j joint number
* @param ref specifies the new ref point
Expand Down Expand Up @@ -434,7 +426,7 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo
*/
virtual bool stop();

// ------- IPositionControl2 declarations. Implementation in IPositionControl2.cpp -------
// ------- IPositionControl2 declarations. Implementation in IPositionControl2Impl.cpp -------

/** Set new reference point for a subset of joints.
* @param joints pointer to the array of joint numbers
Expand Down Expand Up @@ -532,15 +524,6 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo

// ------- IPositionDirect declarations. Implementation in IPositionDirectImpl.cpp -------

/**
* Set position direct mode. This command
* is required to switch control boards to low-level position
* control method.
* @return true/false on success failure
*/
virtual bool setPositionDirectMode();


/** Set new position for a single axis.
* @param j joint number
* @param ref specifies the new ref point
Expand Down Expand Up @@ -569,15 +552,6 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo

// -------- ITorqueControl declarations. Implementation in ITorqueControlImpl.cpp --------

/**
* Set torque control mode. This command
* is required by control boards implementing different
* control methods (e.g. velocity/torque), in some cases
* it can be left empty.
* @return true/false on success/failure
*/
virtual bool setTorqueMode();

/** Get the reference value of the torque for all joints.
* This is NOT the feedback (see getTorques instead).
* @param t pointer to the array of torque values
Expand Down Expand Up @@ -755,16 +729,7 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo
*/
virtual bool setTorqueOffset(int j, double v);

// --------- IVelocityControl Declarations. Implementation in IVelocityControl2.cpp ---------

/**
* Set velocity mode. This command
* is required by control boards implementing different
* control methods (e.g. velocity/torque), in some cases
* it can be left empty.
* @return true/false on success failure
*/
virtual bool setVelocityMode();
// --------- IVelocityControl Declarations. Implementation in IVelocityControl2Impl.cpp ---------

/**
* Start motion at a given speed, single joint.
Expand All @@ -781,7 +746,7 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo
*/
virtual bool velocityMove(const double *sp);

// --------- IVelocityControl2 Declarations. Implementation in IVelocityControl2.cpp ----------
// --------- IVelocityControl2 Declarations. Implementation in IVelocityControl2Impl.cpp ----------

/** Start motion at a given speed for a subset of joints.
* @param n_joint how many joints this command is referring to
Expand Down Expand Up @@ -865,7 +830,7 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo
*/
virtual bool getVelPids(yarp::dev::Pid *pids);

// -----------IInteracionMode Declarations. Implementation in IInteracionMode.cpp --------------
// -----------IInteracionMode Declarations. Implementation in IInteracionModeImpl.cpp --------------
/**
* Get the current interaction mode of the robot, values can be stiff or compliant.
* @param axis joint number
Expand Down Expand Up @@ -980,7 +945,7 @@ class CanBusControlboard : public yarp::dev::DeviceDriver, public yarp::dev::ICo
/** A vector of CAN node objects. */
std::vector< yarp::dev::PolyDriver* > nodes;
std::vector< yarp::dev::IControlLimits2Raw* > iControlLimits2Raw;
std::vector< yarp::dev::IControlModeRaw* > iControlModeRaw;
std::vector< yarp::dev::IControlMode2Raw* > iControlMode2Raw;
std::vector< yarp::dev::IEncodersTimedRaw* > iEncodersTimedRaw;
std::vector< yarp::dev::IPositionControl2Raw* > iPositionControl2Raw;
std::vector< yarp::dev::IPositionDirectRaw* > iPositionDirectRaw;
Expand Down
35 changes: 17 additions & 18 deletions libraries/BodyYarp/CanBusControlboard/DeviceDriverImpl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ bool teo::CanBusControlboard::open(yarp::os::Searchable& config)
//-- Populate the CAN nodes vector.
nodes.resize( ids.size() );
iControlLimits2Raw.resize( nodes.size() );
iControlModeRaw.resize( nodes.size() );
iControlMode2Raw.resize( nodes.size() );
iEncodersTimedRaw.resize( nodes.size() );
iPositionControl2Raw.resize( nodes.size() );
iPositionDirectRaw.resize( nodes.size() );
Expand Down Expand Up @@ -93,9 +93,9 @@ bool teo::CanBusControlboard::open(yarp::os::Searchable& config)
return false;
}

if( !device->view( iControlModeRaw[i] ))
if( !device->view( iControlMode2Raw[i] ))
{
CD_ERROR("[error] Problems acquiring iControlModeRaw interface\n");
CD_ERROR("[error] Problems acquiring iControlMode2Raw interface\n");
return false;
}

Expand Down Expand Up @@ -236,27 +236,26 @@ bool teo::CanBusControlboard::open(yarp::os::Searchable& config)

//-- Set all motor drivers to mode.

if( mode=="position")
{
if( ! this->setPositionMode() )
return false;
}
else if( mode=="velocity")
{
if( ! this->setVelocityMode() )
return false;
}
else if( mode=="torque")
{
if( ! this->setTorqueMode() )
return false;
}
int controlModeVocab = 0;

if( mode=="position" )
controlModeVocab = VOCAB_CM_POSITION;
else if( mode=="velocity" )
controlModeVocab = VOCAB_CM_VELOCITY;
else if( mode=="torque" )
controlModeVocab = VOCAB_CM_TORQUE;
else
{
CD_ERROR("Not prepared for initializing in mode %s.\n",mode.c_str());
return false;
}

for(int i=0; i<nodes.size(); i++)
{
if( ! this->setControlMode(i, controlModeVocab) )
return false;
}

//-- Check the status of each driver.
std::vector<int> tmp( nodes.size() ); // -- creating a "tmp"vector with "nodes" vector size
this->getControlModes( tmp.data() );
Expand Down
62 changes: 13 additions & 49 deletions ...Yarp/CanBusControlboard/IControlMode2.cpp → .../CanBusControlboard/IControlMode2Impl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,80 +7,59 @@
bool teo::CanBusControlboard::setPositionMode(int j)
{
CD_DEBUG("(%d)\n",j);

//-- Check index within range
if ( ! this->indexWithinRange(j) ) return false;

return iControlModeRaw[j]->setPositionModeRaw( 0 );
return setControlMode(j, VOCAB_CM_POSITION);
}

// -----------------------------------------------------------------------------

bool teo::CanBusControlboard::setVelocityMode(int j)
{
CD_DEBUG("(%d)\n",j);

return iControlModeRaw[j]->setVelocityModeRaw( 0 );
return setControlMode(j, VOCAB_CM_VELOCITY);
}

// -----------------------------------------------------------------------------

bool teo::CanBusControlboard::setTorqueMode(int j)
{
CD_DEBUG("(%d)\n",j);

//-- Check index within range
if ( ! this->indexWithinRange(j) ) return false;

return iControlModeRaw[j]->setTorqueModeRaw( 0 );
return setControlMode(j, VOCAB_CM_TORQUE);
}

// -----------------------------------------------------------------------------

bool teo::CanBusControlboard::setImpedancePositionMode(int j)
{
CD_DEBUG("(%d)\n",j);

//-- Check index within range
if ( ! this->indexWithinRange(j) ) return false;

return iControlModeRaw[j]->setImpedancePositionModeRaw( 0 );
return setControlMode(j, VOCAB_CM_IMPEDANCE_POS);
}

// -----------------------------------------------------------------------------

bool teo::CanBusControlboard::setImpedanceVelocityMode(int j)
{
CD_DEBUG("(%d)\n",j);

//-- Check index within range
if ( ! this->indexWithinRange(j) ) return false;

return iControlModeRaw[j]->setImpedanceVelocityModeRaw( 0 );
return setControlMode(j, VOCAB_CM_IMPEDANCE_VEL);
}

// -----------------------------------------------------------------------------

bool teo::CanBusControlboard::setOpenLoopMode(int j)
{
CD_DEBUG("(%d)\n",j);

//-- Check index within range
if ( ! this->indexWithinRange(j) ) return false;

return iControlModeRaw[j]->setOpenLoopModeRaw( 0 );
CD_ERROR("(%d)\n",j); //-- Removed in YARP 2.3.70
return false;
}

// ---------------------- IControlMode2 Related ---------------------------------
// -----------------------------------------------------------------------------

bool teo::CanBusControlboard::getControlMode(int j, int *mode)
{
//CD_INFO("(%d)\n",j); //-- Too verbose in controlboardwrapper2 stream
//CD_DEBUG("(%d)\n",j); //-- Too verbose in controlboardwrapper2 stream

//-- Check index within range
if ( ! this->indexWithinRange(j) ) return false;

return iControlModeRaw[j]->getControlModeRaw( 0, mode );
return iControlMode2Raw[j]->getControlModeRaw( 0, mode );
}

// -----------------------------------------------------------------------------
Expand All @@ -95,7 +74,7 @@ bool teo::CanBusControlboard::getControlModes(int *modes)
return ok;
}

// -----------------------------------------------------------------------------
// ---------------------- IControlMode2 Related ---------------------------------

bool teo::CanBusControlboard::getControlModes(const int n_joint, const int *joints, int *modes)
{
Expand All @@ -113,32 +92,17 @@ bool teo::CanBusControlboard::setControlMode(const int j, const int mode)
{
CD_DEBUG("(%d, %d)\n",j,mode);

bool ok = true;
//-- Check index within range
if ( ! this->indexWithinRange(j) ) return false;

if( mode == VOCAB_CM_POSITION )
ok = setPositionMode(j);
if( mode == VOCAB_CM_VELOCITY )
ok = setVelocityMode(j);
if( mode == VOCAB_CM_TORQUE )
ok = setTorqueMode(j);
if( mode == VOCAB_CM_IMPEDANCE_POS )
ok = setImpedancePositionMode(j);
if( mode == VOCAB_CM_IMPEDANCE_VEL )
ok = setImpedanceVelocityMode(j);
if( mode == VOCAB_CM_OPENLOOP )
ok = setOpenLoopMode(j);

return ok;

return iControlMode2Raw[j]->setControlModeRaw( 0, mode );
}

// -----------------------------------------------------------------------------

bool teo::CanBusControlboard::setControlModes(const int n_joint, const int *joints, int *modes)
{
CD_DEBUG("\n");
CD_DEBUG("(%d)\n",n_joint);

bool ok = true;
for(int j=0; j<n_joint; j++)
Expand Down
0 ...p/CanBusControlboard/IInteractionMode.cpp → ...nBusControlboard/IInteractionModeImpl.cpp
View file
File renamed without changes.
14 changes: 0 additions & 14 deletions .../CanBusControlboard/IPositionControl2.cpp → ...BusControlboard/IPositionControl2Impl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,20 +15,6 @@ bool teo::CanBusControlboard::getAxes(int *axes)

// -----------------------------------------------------------------------------

bool teo::CanBusControlboard::setPositionMode()
{
CD_DEBUG("\n");

bool ok = true;
for(int j=0; j<nodes.size(); j++)
{
ok &= this->setPositionMode(j);
}
return ok;
}

// -----------------------------------------------------------------------------

bool teo::CanBusControlboard::positionMove(int j, double ref)
{
CD_DEBUG("(%d, %f)\n",j,ref);
Expand Down

0 comments on commit 6e0232e

Please sign in to comment.