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; }