bool PlaybackLocomotion::updateModule() {
if( ! playbackThread.isRunning() ) {
CD_DEBUG("playbackThread not running, so stopping module...\n");
yarp::os::Time::delay(1);
this->stopModule();
return true;
}
CD_DEBUG("playbackThread running...\n");
return true;
}
void RecordRateThread::run() {
//-- Make room!!
std::vector< double > vLeftLegAngles( leftLegNumMotors );
double* leftLegAngles = vLeftLegAngles.data();
std::vector< double > vRightArmAngles( rightLegNumMotors );
double* rightLegAngles = vRightArmAngles.data();
leftLegEnc->getEncoders( leftLegAngles );
rightLegEnc->getEncoders( rightLegAngles );
for(int i=0;i<leftLegNumMotors;i++)
fprintf(filePtr,"%f ",leftLegAngles[i]);
for(int i=0;i<rightLegNumMotors;i++)
fprintf(filePtr,"%f ",rightLegAngles[i]);
fprintf(filePtr,"\n");
CD_DEBUG("--> ");
for(int i=0;i<leftLegNumMotors;i++)
CD_DEBUG_NO_HEADER("%f ",leftLegAngles[i]);
CD_DEBUG_NO_HEADER("|");
for(int i=0;i<rightLegNumMotors;i++)
CD_DEBUG_NO_HEADER("%f ",rightLegAngles[i]);
CD_DEBUG_NO_HEADER("\n");
}
bool teo::FakeControlboard::getControlModes(int *modes) {
CD_DEBUG("\n");
bool ok = true;
for(unsigned int i=0; i < axes; i++)
ok &= getControlMode(i,&(modes[i]));
return ok;
}
void CartesianRateThread::run()
{
if (currentState == PTMODE)
{
std::string line;
if (! getline( ifs, line) )
{
this->stop();
CD_INFO("[L:%d] file ended\n", lineCount );
return;
}
CD_DEBUG("[L:%d] %s\n", lineCount,line.c_str() );
lineCount++;
yarp::os::Bottle lineBottle(line); //-- yes, using a bottle to parse a string
//-- xDesired from file
for(int i=0; i<xDesired.size(); i++)
xDesired[i] = lineBottle.get( i ).asDouble();
//-- xDotDesired from file
for(int i=0; i<xDotDesired.size(); i++)
xDotDesired[i] = lineBottle.get( i+xDesired.size() ).asDouble();
//-- qReal from encoders
iEncoders->getEncoders( qReal.data() );
//-- xError = xDesired - xReal, where xReal is fwdKin of qReal
solver->fwdKinError(xDesired,qReal, xError);
//-- control law in cartesian space
for(int i=0; i<xDotCmd.size(); i++)
xDotCmd[i] = xDotDesired[i] + DEFAULT_GAIN * xError[i];
//-- final command to joint space
solver->diffInvKin(qReal,xDotCmd, qDotCmd);
CD_INFO("--> ");
for(int i=0; i<qDotCmd.size(); i++) {
CD_INFO_NO_HEADER("%f ",qDotCmd[i]);
}
CD_INFO_NO_HEADER("[deg/s]\n");
iVelocityControl->velocityMove( qDotCmd.data() );
} //end{if(currentState == PTMODE)}
/*else if (currentState == MOVL)
{
}*/
}
int main(int argc, char *argv[]) {
CD_ERROR("Test CD_ERROR.\n");
CD_WARNING("Test CD_WARNING.\n");
CD_SUCCESS("Test CD_SUCCESS.\n");
CD_INFO("Test CD_INFO.\n");
CD_DEBUG("Test CD_DEBUG.\n\n");
CD_ERROR_NO_HEADER("Test CD_ERROR_NO_HEADER.\n");
CD_WARNING_NO_HEADER("Test CD_WARNING_NO_HEADER.\n");
CD_SUCCESS_NO_HEADER("Test CD_SUCCESS_NO_HEADER.\n");
CD_INFO_NO_HEADER("Test CD_INFO_NO_HEADER.\n");
CD_DEBUG_NO_HEADER("Test CD_DEBUG_NO_HEADER.\n\n");
return 0;
}
void teo::GravityRateThread::run() {
iEncoders->getEncoders( q.data() );
CD_DEBUG("<-- ");
for(int i=0;i<numMotors;i++)
CD_DEBUG_NO_HEADER("%f ",q[i]);
CD_DEBUG_NO_HEADER("[deg]\n");
solver->invDyn(q,t);
if( numMotors > numMotors ) //-- ??????????????????????????
t.resize( numMotors ); //-- Extra motors won't care about torques.
CD_INFO("--> ");
for(int i=0;i<numMotors;i++) {
CD_INFO_NO_HEADER("%f ",t[i]);
}
CD_INFO_NO_HEADER("[Nm]\n");
//--tRA[0] = 0.0; //-- Release... let's do this!
iTorqueControl->setRefTorques( t.data() );
}
bool teo::FakeControlboard::getTargetPositions(const int n_joint, const int *joints, double *refs)
{
CD_DEBUG("\n");
return true;
}
bool teo::FakeControlboard::getTargetPositions(double *refs)
{
CD_DEBUG("\n");
return true;
}
bool teo::FakeControlboard::stop(const int n_joint, const int *joints)
{
CD_DEBUG("\n");
return true;
}
bool teo::FakeControlboard::positionMove(const int n_joint, const int *joints, const double *refs)
{
CD_DEBUG("\n");
// must implement mask!
return positionMove(refs);
}
bool PlaybackManipulation::configure(ResourceFinder &rf) {
int ptModeMs = rf.check("ptModeMs",yarp::os::Value(DEFAULT_PT_MODE_MS),"PT mode miliseconds").asInt();
CD_INFO("Using ptModeMs: %d (default: %d).\n",ptModeMs,int(DEFAULT_PT_MODE_MS));
playbackThread.hold = rf.check("hold");
//-- Open file for logging.
if ( rf.check("log") ) {
playbackThread.log = true;
std::string fileName = rf.check("log",yarp::os::Value(DEFAULT_LOG_NAME),"file name").asString();
CD_INFO("Using log file: %s (default: " DEFAULT_LOG_NAME ").\n",fileName.c_str());
playbackThread.logFilePtr = ::fopen (fileName.c_str(),"w");
if( ! playbackThread.logFilePtr ) {
CD_PERROR("Could not open log file: %s.\n",fileName.c_str());
return false;
}
CD_SUCCESS("Opened log file: %s.\n",fileName.c_str());
} else {
playbackThread.log = false;
}
//-- Open file for reading.
std::string fileName = rf.check("file",Value(DEFAULT_FILE_NAME),"file name").asString();
CD_INFO("Using read file: %s (default: " DEFAULT_FILE_NAME ").\n",fileName.c_str());
playbackThread.ifs.open( fileName.c_str() );
if( ! playbackThread.ifs.is_open() ) {
CD_ERROR("Could not open read file: %s.\n",fileName.c_str());
return false;
}
CD_SUCCESS("Opened file: %s.\n",fileName.c_str());
std::string allIni = rf.findFileByName("../launchManipulation/launchManipulation.ini");
yarp::os::Property allOptions;
if (! allOptions.fromConfigFile(allIni) ) { //-- Put first because defaults to wiping out.
CD_ERROR("Could not configure from \"launchManipulation.ini\".\n");
return false;
}
//CD_SUCCESS("Configuring from %s.\n",allOptions.toString().c_str());
//-- /dev/can0 --
yarp::os::Bottle devCan0 = allOptions.findGroup("devCan0");
CD_DEBUG("%s\n",devCan0.toString().c_str());
yarp::os::Property optionsDevCan0;
optionsDevCan0.fromString(devCan0.toString());
optionsDevCan0.put("device","CanBusControlboard");
optionsDevCan0.put("ptModeMs",ptModeMs);
if (rf.check("home")) optionsDevCan0.put("home",1);
if (rf.check("reset")) optionsDevCan0.put("reset",1);
leftArmDevice.open(optionsDevCan0);
if (!leftArmDevice.isValid()) {
CD_ERROR("leftArmDevice instantiation not worked.\n");
CD_ERROR("Be sure CMake \"ENABLE_BodyYarp_CanBusControlboard\" variable is set \"ON\"\n");
CD_ERROR("\"SKIP_CanBusControlboard is set\" --> should be --> \"ENABLE_CanBusControlboard is set\"\n");
// robotDevice.close(); // un-needed?
return false;
}
//-- /dev/can1 --
yarp::os::Bottle devCan1 = allOptions.findGroup("devCan1");
CD_DEBUG("%s\n",devCan1.toString().c_str());
yarp::os::Property optionsDevCan1;
optionsDevCan1.fromString(devCan1.toString());
optionsDevCan1.put("device","CanBusControlboard");
optionsDevCan1.put("ptModeMs",ptModeMs);
if (rf.check("home")) optionsDevCan1.put("home",1);
if (rf.check("reset")) optionsDevCan1.put("reset",1);
rightArmDevice.open(optionsDevCan1);
if (!rightArmDevice.isValid()) {
CD_ERROR("rightArmDevice instantiation not worked.\n");
CD_ERROR("Be sure CMake \"ENABLE_BodyYarp_CanBusControlboard\" variable is set \"ON\"\n");
CD_ERROR("\"SKIP_CanBusControlboard is set\" --> should be --> \"ENABLE_CanBusControlboard is set\"\n");
// robotDevice.close(); // un-needed?
return false;
}
//-- Configure the thread.
//-- Obtain manipulator interfaces.
if ( ! leftArmDevice.view( playbackThread.leftArmPos ) ) {
CD_ERROR("Could not obtain leftArmPos.\n");
return false;
}
CD_SUCCESS("Obtained leftArmPos.\n");
if ( ! leftArmDevice.view( playbackThread.leftArmPosDirect ) ) {
CD_ERROR("Could not obtain leftArmPosDirect.\n");
return false;
}
CD_SUCCESS("Obtained leftArmPosDirect.\n");
if ( ! leftArmDevice.view( playbackThread.leftArmEncTimed ) ) {
CD_ERROR("Could not obtain leftArmEncTimed.\n");
return false;
}
CD_SUCCESS("Obtained leftArmEncTimed.\n");
if ( ! leftArmDevice.view( playbackThread.leftArmTorque ) ) {
CD_ERROR("Could not obtain leftArmTorque.\n");
return false;
}
CD_SUCCESS("Obtained leftArmTorque.\n");
if ( ! rightArmDevice.view( playbackThread.rightArmPos ) ) {
CD_ERROR("Could not obtain leftArmPos.\n");
return false;
}
CD_SUCCESS("Obtained rightArmPos.\n");
if ( ! rightArmDevice.view( playbackThread.rightArmPosDirect ) ) {
CD_ERROR("Could not obtain rightArmPosDirect.\n");
return false;
}
CD_SUCCESS("Obtained rightArmPosDirect.\n");
if ( ! rightArmDevice.view( playbackThread.rightArmEncTimed ) ) {
CD_ERROR("Could not obtain rightArmEncTimed.\n");
return false;
}
CD_SUCCESS("Obtained rightArmEncTimed.\n");
if ( ! rightArmDevice.view( playbackThread.rightArmTorque ) ) {
CD_ERROR("Could not obtain rightArmTorque.\n");
return false;
}
CD_SUCCESS("Obtained rightArmTorque.\n");
//-- Do stuff.
playbackThread.leftArmPos->getAxes( &(playbackThread.leftArmNumMotors) );
playbackThread.rightArmPos->getAxes( &(playbackThread.rightArmNumMotors) );
CD_INFO("setPositionDirectMode...\n");
playbackThread.leftArmPosDirect->setPositionDirectMode();
playbackThread.rightArmPosDirect->setPositionDirectMode();
//-- Start the thread.
CD_INFO("Start thread...\n");
playbackThread.start();
return true;
}
bool teo::FakeControlboard::getRefSpeeds(const int n_joint, const int *joints, double *spds)
{
CD_DEBUG("\n");
return true;
}
bool teo::FakeControlboard::checkMotionDone(const int n_joint, const int *joints, bool *flags)
{
CD_DEBUG("\n");
return true;
}
bool teo::FakeControlboard::relativeMove(const int n_joint, const int *joints, const double *deltas)
{
CD_DEBUG("\n");
return true;
}
bool TwoCanBusThreeWrappers::configure(ResourceFinder &rf) {
if(rf.check("help")) {
printf("TwoCanBusThreeWrappers options:\n");
printf("\t--help (this help)\t--from [file.ini]\t--context [path]\n");
CD_DEBUG_NO_HEADER("%s\n",rf.toString().c_str());
return false;
}
//-- /dev/can0 --
Bottle devCan0 = rf.findGroup("devCan0");
CD_DEBUG("%s\n",devCan0.toString().c_str());
Property optionsDevCan0;
optionsDevCan0.fromString(devCan0.toString());
deviceDevCan0.open(optionsDevCan0);
if (!deviceDevCan0.isValid()) {
CD_ERROR("deviceDevCan0 instantiation not worked.\n");
return false;
}
//-- /dev/can1 --
Bottle devCan1 = rf.findGroup("devCan1");
CD_DEBUG("%s\n",devCan1.toString().c_str());
Property optionsDevCan1;
optionsDevCan1.fromString(devCan1.toString());
deviceDevCan1.open(optionsDevCan1);
if (!deviceDevCan1.isValid()) {
CD_ERROR("deviceDevCan1 instantiation not worked.\n");
return false;
}
//-- wrapper0 --
Bottle wrapper0 = rf.findGroup("wrapper0");
CD_DEBUG("%s\n",wrapper0.toString().c_str());
Property optionsWrapper0;
optionsWrapper0.fromString(wrapper0.toString());
deviceWrapper0.open(optionsWrapper0);
if (!deviceWrapper0.isValid()) {
CD_ERROR("deviceWrapper0 instantiation not worked.\n");
return false;
}
//-- wrapper1 --
Bottle wrapper1 = rf.findGroup("wrapper1");
CD_DEBUG("%s\n",wrapper1.toString().c_str());
Property optionsWrapper1;
optionsWrapper1.fromString(wrapper1.toString());
deviceWrapper1.open(optionsWrapper1);
if (!deviceWrapper1.isValid()) {
CD_ERROR("deviceWrapper1 instantiation not worked.\n");
return false;
}
//-- wrapper2 --
Bottle wrapper2 = rf.findGroup("wrapper2");
CD_DEBUG("%s\n",wrapper2.toString().c_str());
Property optionsWrapper2;
optionsWrapper2.fromString(wrapper2.toString());
deviceWrapper2.open(optionsWrapper2);
if (!deviceWrapper2.isValid()) {
CD_ERROR("deviceWrapper2 instantiation not worked.\n");
return false;
}
IMultipleWrapper *iWrapper0, *iWrapper1, *iWrapper2;
deviceWrapper0.view(iWrapper0);
deviceWrapper1.view(iWrapper1);
deviceWrapper2.view(iWrapper2);
PolyDriverList list;
list.push(&deviceDevCan0, "devCan0");
list.push(&deviceDevCan1, "devCan1");
iWrapper0->attachAll(list);
iWrapper1->attachAll(list);
iWrapper2->attachAll(list);
return true;
}
bool teo::FakeControlboard::getRefAccelerations(const int n_joint, const int *joints, double *accs)
{
CD_DEBUG("\n");
return true;
}
bool teo::CanBusControlboard::open(Searchable& config) {
std::string mode = config.check("mode",Value(DEFAULT_MODE),"position/velocity mode").asString();
int16_t ptModeMs = config.check("ptModeMs",Value(DEFAULT_PT_MODE_MS),"PT mode miliseconds").asInt();
Bottle ids = config.findGroup("ids").tail(); //-- e.g. 15
Bottle trs = config.findGroup("trs").tail(); //-- e.g. 160
Bottle ks = config.findGroup("ks").tail(); //-- e.g. 0.0706
Bottle maxs = config.findGroup("maxs").tail(); //-- e.g. 360
Bottle mins = config.findGroup("mins").tail(); //-- e.g. -360
Bottle refAccelerations = config.findGroup("refAccelerations").tail(); //-- e.g. 0.575437
Bottle refSpeeds = config.findGroup("refSpeeds").tail(); //-- e.g. 737.2798
Bottle types = config.findGroup("types").tail(); //-- e.g. 15
//-- Initialize the CAN device.
Property canBusOptions;
canBusOptions.fromString(config.toString()); // canDevice, canBitrate
canBusOptions.put("device","CanBusHico");
canBusDevice.open(canBusOptions);
if( ! canBusDevice.isValid() ){
CD_ERROR("canBusDevice instantiation not worked.\n");
return false;
}
canBusDevice.view(iCanBus);
//-- Start the reading thread (required for checkMotionDoneRaw).
this->Thread::start();
//-- Populate the CAN nodes vector.
nodes.resize( ids.size() );
iControlLimitsRaw.resize( nodes.size() );
iControlModeRaw.resize( nodes.size() );
iEncodersTimedRaw.resize( nodes.size() );
iPositionControlRaw.resize( nodes.size() );
iPositionDirectRaw.resize( nodes.size() );
iTorqueControlRaw.resize( nodes.size() );
iVelocityControlRaw.resize( nodes.size() );
iCanBusSharer.resize( nodes.size() );
for(int i=0; i<nodes.size(); i++)
{
if(types.get(i).asString() == "")
CD_WARNING("Argument \"types\" empty at %d.\n",i);
//-- Create CAN node objects with a pointer to the CAN device, its id and tr (these are locally stored parameters).
Property options;
options.put("device",types.get(i).asString()); //-- "TechnosoftIpos", "LacqueyFetch"
options.put("canId",ids.get(i).asInt());
options.put("tr",trs.get(i).asDouble());
options.put("min",mins.get(i).asDouble());
options.put("max",maxs.get(i).asDouble());
options.put("k",ks.get(i).asDouble());
options.put("refAcceleration",refAccelerations.get(i).asDouble());
options.put("refSpeed",refSpeeds.get(i).asDouble());
options.put("ptModeMs",ptModeMs);
PolyDriver* driver = new PolyDriver(options);
//-- Fill a map entry ( drivers.size() if before push_back, otherwise do drivers.size()-1).
//-- Just "i" if resize already performed.
idxFromCanId[ ids.get(i).asInt() ] = i;
//-- Push the motor driver on to the vectors.
nodes[i] = driver;
driver->view( iControlLimitsRaw[i] );
driver->view( iControlModeRaw[i] );
driver->view( iEncodersTimedRaw[i] );
driver->view( iPositionControlRaw[i] );
driver->view( iPositionDirectRaw[i] );
driver->view( iTorqueControlRaw[i] );
driver->view( iVelocityControlRaw[i] );
driver->view( iCanBusSharer[i] );
//-- Associate absolute encoders to motor drivers
if( types.get(i).asString() == "CuiAbsolute" ) {
int driverCanId = ids.get(i).asInt() - 100;
iCanBusSharer[ idxFromCanId[driverCanId] ]->setIEncodersTimedRawExternal( iEncodersTimedRaw[i] );
}
//-- Aditionally sets initial parameters on physical motor drivers.
iCanBusSharer[i]->setCanBusPtr( iCanBus );
}
//-- 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;
} else {
CD_ERROR("Not prepared for initializing in mode %s.\n",mode.c_str());
return false;
}
//-- Check the status of each driver.
std::vector<int> tmp( nodes.size() );
this->getControlModes( tmp.data() );
//-- Initialize the drivers: start (0.1) ready (0.1) on (2) enable. Wait between each step.
for(int i=0; i<nodes.size(); i++)
{
if( ! iCanBusSharer[i]->start() )
return false;
}
yarp::os::Time::delay(0.1);
for(int i=0; i<nodes.size(); i++)
{
if( ! iCanBusSharer[i]->readyToSwitchOn() )
return false;
}
yarp::os::Time::delay(0.1);
for(int i=0; i<nodes.size(); i++)
{
if( ! iCanBusSharer[i]->switchOn() )
return false;
}
yarp::os::Time::delay(2);
for(int i=0; i<nodes.size(); i++)
{
if( ! iCanBusSharer[i]->enable() )
return false;
}
if( config.check("home") ) {
CD_DEBUG("Moving motors to zero.\n");
for(int i=0; i<nodes.size(); i++)
{
if ( ! iPositionControlRaw[i]->positionMoveRaw(0,0) )
return false;
}
for(int i=0; i<nodes.size(); i++)
{
bool motionDone = false;
while( ! motionDone ) {
yarp::os::Time::delay(0.5); //-- [s]
CD_DEBUG("Moving %d to zero...\n",i);
if( ! iPositionControlRaw[i]->checkMotionDoneRaw(0,&motionDone) )
return false;
}
}
CD_DEBUG("Moved motors to zero.\n");
}
if( config.check("reset") ) {
CD_DEBUG("Forcing encoders to zero.\n");
if ( ! this->resetEncoders() )
return false;
}
return true;
}
