void YouBotDiagnostics::getAllTopologicalInformation()
{
bool reset = true;
unsigned jointNo = 0;
unsigned jointCfg = 0;
//joint konfiguration variables
JointName jName;
GearRatio gearRatio;
EncoderTicksPerRound ticksPerRound;
InverseMovementDirection inverseDir;
JointLimits jLimits;
FirmwareVersion firmwareTypeVersion;
ticksPerRound.setParameter(4000);
int controllerType;
string firmwareVersion;
*outputFile << "----- Topological Information of all slaves: -----" << endl;
for (int i = 0, slaveCtr = 1; i < etherCATSlaves.size(); i++, slaveCtr++)
{
//formatted output:
*outputFile << "slave no " << setw(2) << slaveCtr << ":\t" << slaveTypMap[etherCATSlaves[i].name];
// if( slaveTypMap[etherCATSlaves[i].name] == "BASE-Master")
// *outputFile << ": " << etherCATSlaves[i].name;
// else
*outputFile << ":\t" << etherCATSlaves[i].name;
if ( slaveTypMap[etherCATSlaves[i].name] == "BASE-Master" || slaveTypMap[etherCATSlaves[i].name] == "ARM-Master")
*outputFile << "\t\t--> parent: ";
else
*outputFile << "\t--> parent: ";
*outputFile << setw(2) << etherCATSlaves[i].parent << "\t--> connections: " << setw(2) << (int) etherCATSlaves[i].topology << endl;
//create the joint map
if (reset)
jointCfg = 1;
else
jointCfg++;
if ( !strcmp( etherCATSlaves[i].name, BASE_MASTER_BOARD) )
{
reset = true;
noOfBaseMasterBoards++;
motorActivity[slaveCtr] = true;
}
if ( !strcmp( etherCATSlaves[i].name, ARM_MASTER_BOARD ) )
{
reset = true;
noOfArmMasterBoards++;
motorActivity[slaveCtr] = true;
}
if ( !strcmp( etherCATSlaves[i].name, BASE_CONTROLLER ) )
{
noOfBaseSlaveControllers++;
jointNo++;
reset = false;
TopologyMap::iterator it = youBotJointMap.insert( pair<int, JointTopology>( slaveCtr, JointTopology() ) ).first;
it->second.relatedUnit = string("BASE");
it->second.relatedMasterBoard = string(BASE_MASTER_BOARD);
it->second.controllerTyp = string(BASE_CONTROLLER);
it->second.masterBoardNo = noOfBaseMasterBoards;
it->second.slaveNo = slaveCtr;
it->second.jointTopologyNo = jointNo;
it->second.jointConfigNo = jointCfg;
it->second.joint = new YouBotJoint(jointNo);
it->second.joint->getConfigurationParameter(firmwareTypeVersion);
firmwareTypeVersion.getParameter(controllerType, firmwareVersion);
it->second.firmwareVersion = atof(firmwareVersion.c_str());
stringstream name;
name << "joint_" << jointNo;
jName.setParameter( name.str() );
gearRatio.setParameter(364.0/9405.0);
inverseDir.setParameter(false);
it->second.joint->setConfigurationParameter(jName);
it->second.joint->setConfigurationParameter(gearRatio);
it->second.joint->setConfigurationParameter(ticksPerRound);
it->second.joint->setConfigurationParameter(inverseDir);
}
if( !strcmp( etherCATSlaves[i].name, ARM_CONTROLLER ) )
{
noOfArmSlaveControllers++;
jointNo++;
reset = false;
TopologyMap::iterator it = youBotJointMap.insert( pair<int, JointTopology>( slaveCtr, JointTopology() ) ).first;
it->second.relatedUnit = string("MANIPULATOR");
it->second.relatedMasterBoard = string(ARM_MASTER_BOARD);
it->second.controllerTyp = string(ARM_CONTROLLER);
it->second.masterBoardNo = noOfArmMasterBoards;
it->second.slaveNo = slaveCtr;
it->second.jointTopologyNo = jointNo;
it->second.jointConfigNo = jointCfg;
it->second.joint = new YouBotJoint(jointNo);
it->second.joint->getConfigurationParameter(firmwareTypeVersion);
firmwareTypeVersion.getParameter(controllerType, firmwareVersion);
it->second.firmwareVersion = atof(firmwareVersion.c_str());
stringstream name;
name << "joint_" << jointNo;
jName.setParameter( name.str() );
switch (jointCfg)
{
case 1:
gearRatio.setParameter(1.0/156.0);
inverseDir.setParameter(true);
jLimits.setParameter(-580000, -1000, true);
break;
case 2:
gearRatio.setParameter(1.0/156.0);
inverseDir.setParameter(true);
jLimits.setParameter(-260000, -1000, true);
break;
case 3:
gearRatio.setParameter(1.0/100.0);
inverseDir.setParameter(true);
jLimits.setParameter(-320000, -1000, true);
break;
case 4:
gearRatio.setParameter(1.0/ 71.0);
inverseDir.setParameter(true);
jLimits.setParameter(-155000, -1000, true);
break;
case 5:
gearRatio.setParameter(1.0/ 71.0);
inverseDir.setParameter(true);
jLimits.setParameter(-255000, -1000, true);
break;
}
it->second.joint->setConfigurationParameter(jName);
it->second.joint->setConfigurationParameter(ticksPerRound);
it->second.joint->setConfigurationParameter(inverseDir);
it->second.joint->setConfigurationParameter(jLimits);
it->second.joint->setConfigurationParameter(gearRatio);
}
}
*outputFile << separator;
*outputFile << "Number of ALL slaves: " << etherCATSlaves.size() << endl;
*outputFile << "Number of JOINT slaves: " << youBotJointMap.size() << endl;
*outputFile << "Number of BaseMasterBoards: " << noOfBaseMasterBoards << endl;
*outputFile << "Number of BaseSlaveControllers: " << noOfBaseSlaveControllers << endl;
*outputFile << "Number of ArmMasterBoards: " << noOfArmMasterBoards << endl;
*outputFile << "Number of ArmSlaveControllers: " << noOfArmSlaveControllers << endl;
*outputFile << separator;
}
void YouBotManipulator::initializeJoints() {
// Bouml preserved body begin 00068071
// LOG(info) << "Initializing Joints";
//enable overriding the number of joints
if (configfile->keyExists("JointTopology", "NumberJoints"))
configfile->readInto(numberArmJoints, "JointTopology", "NumberJoints");
//get number of slaves
unsigned int noSlaves = ethercatMaster.getNumberOfSlaves();
if (noSlaves < numberArmJoints) {
throw std::runtime_error("Not enough ethercat slaves were found to create a YouBotManipulator!");
}
unsigned int slaveNumber = 0;
for (unsigned int i = 1; i <= numberArmJoints; i++) {
std::stringstream jointConfigNameStream;
jointConfigNameStream << "ManipulatorJoint" << i;
std::string jointConfigName = jointConfigNameStream.str();
configfile->readInto(slaveNumber, "JointTopology", jointConfigName);
if (slaveNumber <= noSlaves) {
joints.push_back(new YouBotJoint(slaveNumber));
} else {
throw std::out_of_range("The ethercat slave number is not available!");
}
}
//Configure Joint Parameters
std::string jointName;
JointName jName;
GearRatio gearRatio;
EncoderTicksPerRound ticksPerRound;
InverseMovementDirection inverseDir;
double gearRatio_numerator = 0;
double gearRatio_denominator = 1;
FirmwareVersion firmwareTypeVersion;
TorqueConstant torqueConst;
double trajectory_p=0, trajectory_i=0, trajectory_d=0, trajectory_imax=0, trajectory_imin=0;
for (unsigned int i = 0; i < numberArmJoints; i++) {
std::stringstream jointNameStream;
jointNameStream << "Joint_" << i + 1;
jointName = jointNameStream.str();
joints[i].getConfigurationParameter(firmwareTypeVersion);
std::string version;
int controllerType;
std::string firmwareVersion;
firmwareTypeVersion.getParameter(controllerType, firmwareVersion);
string name;
configfile->readInto(name, jointName, "JointName");
jName.setParameter(name);
LOG(info) << name << "\t Controller Type: " << controllerType << " Firmware version: " << firmwareVersion;
if (this->controllerType != controllerType && alternativeControllerType != controllerType) {
std::stringstream ss;
ss << "The youBot manipulator motor controller have to be of type: " << this->controllerType << " or " << alternativeControllerType;
throw std::runtime_error(ss.str().c_str());
}
//check if firmware is supported
bool isfirmwareSupported = false;
for(unsigned int d = 0; d < supportedFirmwareVersions.size(); d++){
if(this->supportedFirmwareVersions[d] == firmwareVersion){
isfirmwareSupported = true;
break;
}
}
if(!isfirmwareSupported){
throw std::runtime_error("Unsupported firmware version: " + firmwareVersion);
}
if(this->actualFirmwareVersionAllJoints == ""){
this->actualFirmwareVersionAllJoints = firmwareVersion;
}
if(!(firmwareVersion == this->actualFirmwareVersionAllJoints)){
throw std::runtime_error("All joints must have the same firmware version!");
}
configfile->readInto(gearRatio_numerator, jointName, "GearRatio_numerator");
configfile->readInto(gearRatio_denominator, jointName, "GearRatio_denominator");
gearRatio.setParameter(gearRatio_numerator / gearRatio_denominator);
int ticks;
configfile->readInto(ticks, jointName, "EncoderTicksPerRound");
ticksPerRound.setParameter(ticks);
double torqueConstant;
configfile->readInto(torqueConstant, jointName, "TorqueConstant_[newton_meter_divided_by_ampere]");
torqueConst.setParameter(torqueConstant);
bool invdir = false;
configfile->readInto(invdir, jointName, "InverseMovementDirection");
inverseDir.setParameter(invdir);
joints[i].setConfigurationParameter(jName);
joints[i].setConfigurationParameter(gearRatio);
joints[i].setConfigurationParameter(ticksPerRound);
joints[i].setConfigurationParameter(torqueConst);
joints[i].setConfigurationParameter(inverseDir);
//Joint Trajectory Controller
if(ethercatMaster.isThreadActive()){
configfile->readInto(trajectory_p, jointName, "trajectory_controller_P");
configfile->readInto(trajectory_i, jointName, "trajectory_controller_I");
configfile->readInto(trajectory_d, jointName, "trajectory_controller_D");
configfile->readInto(trajectory_imax, jointName, "trajectory_controller_I_max");
configfile->readInto(trajectory_imin, jointName, "trajectory_controller_I_min");
joints[i].trajectoryController.setConfigurationParameter(trajectory_p, trajectory_i, trajectory_d, trajectory_imax, trajectory_imin);
joints[i].trajectoryController.setEncoderTicksPerRound(ticks);
joints[i].trajectoryController.setGearRatio(gearRatio_numerator / gearRatio_denominator);
joints[i].trajectoryController.setInverseMovementDirection(invdir);
ethercatMasterWithThread->registerJointTrajectoryController(&(joints[i].trajectoryController), joints[i].getJointNumber());
}
}
configfile->readInto(useGripper, "Gripper", "EnableGripper");
if(useGripper){
//Initializing Gripper
configfile->readInto(slaveNumber, "JointTopology", "ManipulatorJoint5");
this->gripper.reset(new YouBotGripper(slaveNumber));
BarSpacingOffset barOffest;
MaxTravelDistance maxDistance;
MaxEncoderValue maxEncoder;
GripperBarName BarName;
double dummy = 0;
int controllerType;
double firmwareVersion;
string barname;
GripperFirmwareVersion gripperVersion;
this->gripper->getConfigurationParameter(gripperVersion);
gripperVersion.getParameter(controllerType, firmwareVersion);
LOG(info) << "Gripper" << "\t\t Controller Type: " << controllerType << " Firmware version: " << firmwareVersion;
// Gripper Bar 1
configfile->readInto(barname, "GripperBar1", "BarName");
BarName.setParameter(barname);
this->gripper->getGripperBar1().setConfigurationParameter(BarName);
configfile->readInto(dummy, "GripperBar1", "BarSpacingOffset_[meter]");
barOffest.setParameter(dummy * meter);
this->gripper->getGripperBar1().setConfigurationParameter(barOffest);
configfile->readInto(dummy, "GripperBar1", "MaxTravelDistance_[meter]");
maxDistance.setParameter(dummy * meter);
this->gripper->getGripperBar1().setConfigurationParameter(maxDistance);
int maxenc = 0;
configfile->readInto(maxenc, "GripperBar1", "MaxEncoderValue");
maxEncoder.setParameter(maxenc);
this->gripper->getGripperBar1().setConfigurationParameter(maxEncoder);
int stallThreshold = 0;
configfile->readInto(stallThreshold, "GripperBar1", "StallGuard2Threshold");
StallGuard2Threshold threshold;
threshold.setParameter(stallThreshold);
this->gripper->getGripperBar1().setConfigurationParameter(threshold);
bool stallGuardFilter = false;
configfile->readInto(stallGuardFilter, "GripperBar1", "StallGuard2FilterEnable");
StallGuard2FilterEnable filter;
filter.setParameter(stallGuardFilter);
this->gripper->getGripperBar1().setConfigurationParameter(filter);
// Gripper Bar 2
configfile->readInto(barname, "GripperBar2", "BarName");
BarName.setParameter(barname);
this->gripper->getGripperBar2().setConfigurationParameter(BarName);
configfile->readInto(dummy, "GripperBar2", "BarSpacingOffset_[meter]");
barOffest.setParameter(dummy * meter);
this->gripper->getGripperBar2().setConfigurationParameter(barOffest);
configfile->readInto(dummy, "GripperBar2", "MaxTravelDistance_[meter]");
maxDistance.setParameter(dummy * meter);
this->gripper->getGripperBar2().setConfigurationParameter(maxDistance);
configfile->readInto(maxenc, "GripperBar2", "MaxEncoderValue");
maxEncoder.setParameter(maxenc);
this->gripper->getGripperBar2().setConfigurationParameter(maxEncoder);
configfile->readInto(stallThreshold, "GripperBar2", "StallGuard2Threshold");
threshold.setParameter(stallThreshold);
this->gripper->getGripperBar2().setConfigurationParameter(threshold);
configfile->readInto(stallGuardFilter, "GripperBar2", "StallGuard2FilterEnable");
filter.setParameter(stallGuardFilter);
this->gripper->getGripperBar2().setConfigurationParameter(filter);
}
return;
// Bouml preserved body end 00068071
}
