int main( )
{
FPGroup G;
cout << "Enter nilpotentcy class first then group ";
int NilpotentcyClass;
cin >> NilpotentcyClass;
Chars errMsg = cin >> G;
if (errMsg.length()>0)
return 1;
NilpotentGroup ng(G.namesOfGenerators(),
NilpotentcyClass,makeVectorOf(G.getRelators()));
ng.initialize();
VectorOf<Word> vw;
Word w;
for (int i=1;true;i++){
cout << endl << "Enter the "<<i<<" generator of subgroup"<< endl;
cout << "Empty word to finish: ";
w = G.readWord(cin,errMsg);
if (w.length()==0)
break;
if (errMsg.length()>0)
return 1;
vw.append(w);
}
SGOfNilpotentGroup sg(ng,vw);
sg.initBasis();
sg.printBasis(cout);
cout << "The Hirsch number :" << sg.theHirschNumber() << endl;
cout << "Index in parent group :" << sg.index() << endl;
cout << "Is Trivial :" << sg.isTrivial() << endl;
cout << "Is Central :" << sg.isCentral() << endl;
cout << "Is normal :" << sg.isNormal() << endl;
cout << "Is abelian :" << sg.isAbelian() << endl;
cout << "Subgroup class :" << sg.subgroupClass() << endl;
cout << "Generators of normal closure :" << endl;
vw = sg.normalClosureGens();
for (int i=0;i<vw.length();i++){
G.printWord(cout,vw[i]);
cout << endl;
}
PresentationForSNG sgp = sg.makePresentation();
cout << "Presentation of subgroup :" << endl;
sgp.print(cout);
cout << endl << "Enter the word :";
errMsg = "";
w = G.readWord(cin,errMsg);
if (errMsg.length()>0){
cout << errMsg;
return 1;
}
cout << endl << "Does subgroup contain the word :" << sg.contains(w) << endl;
PolyWord result;
if (sg.decompose(ng.decompose(w),result))
cout << "Decomposition in sg basis :" << sg.asDecomposition(result);
else
cout << "Subgroup does not contain this word";
}
// can be invoced only after the cyclic decomposition has been computed
bool AbelianEquationsSolver::root( Word& w , int n ) const
{
AbelianGroup A1 = A.getCanonicalSmithPresentation();
FPGroup G = A1.getFPGroup();
AbelianWord nw = A.oldInAbelianForm( w );
nw = A.oldToNewGens( nw );
Word ans;
VectorOf<Integer> powers = nw.getPowers();
for( int i = 0 ; i < powers.length() ; i++ )
{
Generator g( i + 1 );
int ord = A1.orderOfElt( g ).as_long();
int pow = powers[i].as_long();
if( pow )
if( !ord )
{
if( pow % n )
return no;
else
for( int j = 0 ; j < abs(pow / n) ; j++ )
ans *= ( (pow / n) > 0 ) ? g : inv(g);
}
else
if( pow % n )
{
ord = ( ord > 0 ) ? ord : -ord;
int x;
for( x = -ord ; x < ord ; x++ )
if( ! ( (n * x - pow) % ord ) )
break;
for( int j = 0 ; j < abs( x ) ; j++ )
ans *= ( x > 0 ) ? g : inv(g);
}
else
for( int j = 0 ; j < abs(pow / n) ; j++ )
ans *= ( (pow / n) > 0 ) ? g : inv(g);
}
nw = AbelianWord( G.numberOfGenerators() , ans );
nw = A.newToOldGens( nw );
w = nw.getWord();
return yes;
}
AbelianEquationsSolver::AbelianEquationsSolver( const AbelianGroup& a ,
const VectorOf<Word>& v ,
int numOfVar )
: rawA( a ),
A( FPGroup() ),
rawSystem( v ),
system( v.length() ),
b( v.length() ),
x( numOfVar ),
torsion( numOfVar ),
params( numOfVar ),
numberOfVariables( numOfVar ),
sysRank( 0 ),
haveSol( -1 )
{
FPGroup G( a.getFPGroup() );
VectorOf<Chars> q( G.numberOfGenerators() - numberOfVariables );
for( int i = numberOfVariables ; i < G.numberOfGenerators() ; i++ )
q[ i - numberOfVariables ] = (G.namesOfGenerators())[i];
if( G.getRelators().cardinality() )
{
SetOf<Word> s = G.getRelators();
SetIterator<Word> I(s);
SetOf<Word> news;
while( !I.done() )
{
Word w = I.value();
for( int j = 0 ; j < w.length() ; j++ )
{
int p = Generator( w[j] ).hash();
if( p > 0 )
w[j] = Generator( p - numberOfVariables );
else
w[j] = Generator( p + numberOfVariables );
}
news.adjoinElement( w );
I.next();
}
FPGroup G1( q , news );
A = AbelianGroup( G1 );
}
else
A = AbelianGroup( FPGroup(q) );
}
void handHandler(const bool b)
{
if (b)
{
if (s1==idle)
s1=triggered;
else if (s1==triggered)
{
if (++c1*getPeriod()>0.5)
{
imod->setControlModes(joints.size(),joints.getFirst(),modes.getFirst());
s1=running;
}
}
else
ivel->velocityMove(joints.size(),joints.getFirst(),vels.data());
}
else
{
if (s1==triggered)
vels=-1.0*vels;
if (c1!=0)
ivel->stop(joints.size(),joints.getFirst());
s1=idle;
c1=0;
}
}
VectorOf<Chars> PresentationParser::parseGeneratorList( Chars& errMesg )
{
VectorOf<Chars> result;
if ( curToken == INIT ) getToken();
while ( curToken == GENERATOR ) {
Chars name(tokenName);
// Check for duplication and presence of inverses.
if ( result.indexOf(name) >= 0 ) {
parseError("Duplicate generator");
errMesg = parseErrorMessage;
return result;
} else {
invertName(tokenName);
if ( result.indexOf(Chars(tokenName)) >= 0 ) {
parseError("Duplicate generator: formal inverse");
errMesg = parseErrorMessage;
return result;
}
}
result.append(name); // Ok, it's inefficient, but time is $$
getToken();
if ( curToken == COMMA ) {
getToken();
if ( curToken == DOT ) {
if (!getGeneratorRange(name,result,errMesg))
return result;
} else {
if ( curToken != GENERATOR ) {
parseError("Expected a generator here");
errMesg = parseErrorMessage;
return result;
}
}
}
}
return result;
}
Subgroup& Subgroup::deleteGenerator( const Word& w ) {
VectorOf<Word> theGenerators = look()->theGenerators;
int i = theGenerators.indexOf(w);
if ( i == -1 )
error("attempt to delete nonexistent word in Subgroup::deleteGenerator");
else {
int len = theGenerators.length();
VectorOf<Word> temp(len - 1);
int k = 0;
for( int j = 0; j < len; ++j )
if ( j != i ) temp[k++] = theGenerators[j];
change()->theGenerators = temp;
enhance()->theOrder = -1;
// @stc not yet correct: the order does not necessarily become unknown
return *this;
}
}
bool imuIdentifierThread::goHome()
{
setHeadCtrlModes("position");
// Vector pos0(6,0.0);
// iposH -> positionMove(pos0.data());
VectorOf<int> jointsToSet;
jointsToSet.push_back(0);
jointsToSet.push_back(1);
jointsToSet.push_back(2);
Vector poss(3,0.0);
iposH -> positionMove(jointsToSet.size(),
jointsToSet.getFirst(),
poss.data());
return true;
}
void testToString()
{
{
report(0, "testing toString int");
bool ok = true;
VectorOf<int> vec;
std::string strToCheck = "0 1 2 3 4 5 6 7 8 9";
for (size_t i=0; i<10; i++)
{
vec.push_back(i);
}
ok = vec.toString() == strToCheck;
checkTrue(ok, "string correctly formatted");
}
{
report(0, "testing toString double");
bool ok = true;
VectorOf<double> vec;
std::string strToCheck = " 0.000000\t 1.000000\t 2.000000\t 3.000000\t 4.000000\t"
" 5.000000\t 6.000000\t 7.000000\t 8.000000\t 9.000000";
for (size_t i=0; i<10; i++)
{
vec.push_back(i);
}
ok = vec.toString() == strToCheck;
checkTrue(ok, "string correctly formatted");
}
}
void Controller::setJointsCtrlMode()
{
if (jointsToSet.size()==0)
return;
VectorOf<int> modes;
for (size_t i=0; i<jointsToSet.size(); i++)
{
if (jointsToSet[i]<eyesJoints[0])
{
if (neckPosCtrlOn)
modes.push_back(VOCAB_CM_POSITION_DIRECT);
else
modes.push_back(VOCAB_CM_VELOCITY);
}
else
modes.push_back(VOCAB_CM_MIXED);
}
modHead->setControlModes(jointsToSet.size(),jointsToSet.getFirst(),
modes.getFirst());
}
void AbelianSGPresentationRep::makeFile( )
{
File file;
VectorOf<AbelianWord> cyclicGens = newToOldGens();
int numofnewgens = cyclicGens.length();
int rank = rankOfFreeAbelianFactor();
char s[10];
Word tmpWord;
file.setColor(titleColor);
file << "New generators of the subgroup expressed in terms of the given generators:" << endl;
file.setColor(mainColor);
for( int i = 0; i < rank; ++i ) {
file << "f" << i+1 << " = ";
tmpWord = fromSGPGensToSGGens(cyclicGens[i].getWord());
theParent.getFPGroup().printWord(file,theParent.oldInAbelianForm(tmpWord).getWord() );
file << endl;
}
for( int i = rank; i < numofnewgens; ++i ) {
file << "t" << i - rank + 1 << " = ";
tmpWord = fromSGPGensToSGGens(cyclicGens[i].getWord());
theParent.getFPGroup().printWord(file,theParent.oldInAbelianForm(tmpWord).getWord() );
file << endl;
}
file << endl;
file.setColor(titleColor);
file << "Given generators expressed in terms of the generators for the canonical decomposition of the subgroup:" << endl;
file.setColor(mainColor);
for( int i = 0; i < theSGGens.length(); ++i ) {
theParent.getFPGroup().printWord( file,theSGGens[i] );
file << " = ";
if (fromSGGensToSGPGens(theSGGens[i],tmpWord))
printWordInNewGens(file,oldToNewGens(oldInAbelianForm(tmpWord)));
file << endl;
}
file << end;
theFileName = file.getFileName();
}
VectorOf<Word> SGOfFreeNilpotentGroupRep::normalClosureBasis() const {
//@ep I have found that the process is slower with the full basis,
// so I form it from the scratch.
MalcevSet newBasis(theGenerators, theParentGroup.collector());
newBasis.makeNormalClosure();
// The basis is found. Now we try to reduce it to make further
// processing easier.
VectorOf<Word> basis = newBasis.getCommutatorWords();
// First, we reduce words in terms of basic commutators.
// We could proceed without this step, but it helps
// to reduce the second one greatly.
FreeGroup F( theParentGroup.theHirschNumber() );
basis = F.nielsenBasis(basis);
// Convert words in terms of basic commutators to group
// generators.
for(int i = 0; i < basis.length(); i++) {
PolyWord pw = basis.val(i);
basis[i] = theParentGroup.commutators().wordForm().toWord(pw);
}
// Now reduce this vector
FreeGroup F1( theParentGroup.numberOfGenerators() );
basis = F1.nielsenBasis(basis);
return basis;
}
bool reactCtrlThread::areJointsHealthyAndSet(VectorOf<int> &jointsToSet,
const string &_p, const string &_s)
{
VectorOf<int> modes;
if (_p=="arm")
{
modes=encsA->size();
imodA->getControlModes(modes.getFirst());
}
else if (_p=="torso")
{
modes=encsT->size();
imodT->getControlModes(modes.getFirst());
}
else
return false;
for (size_t i=0; i<modes.size(); i++)
{
if ((modes[i]==VOCAB_CM_HW_FAULT) || (modes[i]==VOCAB_CM_IDLE))
return false;
if (_s=="velocity")
{
if (modes[i]!=VOCAB_CM_MIXED || modes[i]!=VOCAB_CM_VELOCITY)
jointsToSet.push_back(i);
}
else if (_s=="position")
{
if (modes[i]!=VOCAB_CM_MIXED || modes[i]!=VOCAB_CM_POSITION)
jointsToSet.push_back(i);
}
}
return true;
}
bool close()
{
iarm->stopControl();
iarm->restoreContext(startup_context);
drvCart.close();
ivel->stop(joints.size(),joints.getFirst());
for (size_t i=0; i<modes.size(); i++)
modes[i]=VOCAB_CM_POSITION;
imod->setControlModes(joints.size(),joints.getFirst(),modes.getFirst());
drvHand.close();
if (simulator)
{
Bottle cmd,reply;
cmd.addString("world");
cmd.addString("del");
cmd.addString("all");
simPort.write(cmd,reply);
simPort.close();
}
if (gaze)
{
igaze->stopControl();
drvGaze.close();
}
igeo->stopFeedback();
igeo->setTransformation(eye(4,4));
drvGeomagic.close();
forceFbPort.close();
return true;
}
// put the words to the set
MalcevSet::MalcevSet(const VectorOf<Word>& v, const NGCollector& nc)
: isBasis(false), isNormal(dontknow), theCollector(nc)
{
for(int i = 0; i < v.length(); i++)
addWord( v.val(i) );
}
//@njz:default value removed. defined in .h file
//void AbelianEquationsSolver::findSolutions( File& file , bool out = true )
void AbelianEquationsSolver::findSolutions( File& file , bool out)
//
{
haveSol = 0;
if( !A.haveCyclicDecomposition() )
A.computeCyclicDecomposition();
if( !A.havePrimaryDecomposition() )
A.findPrimaryBasis();
if( !rawA.haveCyclicDecomposition() )
rawA.computeCyclicDecomposition();
if( !rawA.havePrimaryDecomposition() )
rawA.findPrimaryBasis();
makeSystem();
if( out )
{
if( system.length() > 1 )
file << "The system of equations: " << endl << endl;
else
file << "The equation: " << endl << endl;
printRawSystem( file );
file << endl << "can be transformed to the one: " << endl << endl;
printSystem( file );
if( system.length() > 1 )
file << endl << "Finding solutions of this system ..." << endl << endl;
else
file << endl << "Finding solutions of this equation ..." << endl << endl;
}
//@njz
// int **matrix = new (int*)[ system.length() ];
int **matrix = new int*[ system.length() ];
//
int i,j,k,i1,j1;
for( i = 0 ; i < system.length() ; i++ )
{
matrix[i] = new int[ numberOfVariables ];
for( j = 0 ; j < numberOfVariables ; j++ )
{
Integer u = (AbelianWord( numberOfVariables , system[i] ))[j];
matrix[i][j] = u.as_long();
}
}
VectorOf< VectorOf<int> > transform;
VectorOf<int> trans(3);
// diagonalization of the matrix
sysRank = 0;
for( i = 0 ; ( i < numberOfVariables && i < system.length() ) ; i++ )
{
bool flag = false;
for( j = i ; j < system.length() ; j++ )
{
for( k = i ; k < numberOfVariables ; k++ )
if( matrix[j][k] )
{
flag = true;
break;
}
if( flag ) break;
}
if( k == numberOfVariables && j == system.length() )
{
if( sysRank )
break;
int q;
for( q = 0 ; q < b.length() ; q++ )
if( !A.isTrivial( Word( b[q] ) ) )
{
haveSol = -1;
if( out )
if( system.length() > 1 )
file << "while computing the canonical form of the system it was found that this system has no solutions." << endl;
else
file << "while computing the canonical form of the equation it was found that this equation has no solutions." << endl;
return;
}
haveSol = 0;
if( out )
if( system.length() > 1 )
file << "while computing the canonical form of the system it was found that this system has all group as a set of solutions." << endl;
else
file << "while computing the canonical form of the equation it was found that this equation has all group as a set of solutions." << endl;
return;
}
int *tmp = matrix[i];
matrix[i] = matrix[j];
matrix[j] = tmp;
Word r = b[i];
b[i] = b[j];
b[j] = r;
for( j = 0 ; j < system.length() ; j++ )
{
int t = matrix[j][i];
matrix[j][i] = matrix[j][k];
matrix[j][k] = t;
}
if( i != k )
{
trans[0] = i;
trans[1] = k;
trans[2] = 0;
transform.append( trans );
}
while( true )
{
bool check;
bool flag;
int z;
bool done = false;
int count = i + 1;
while( !done )
{
for( j = count ; j < system.length() ; j++ )
if( matrix[j][i] && abs(matrix[i][i]) != abs(matrix[j][i]) )
break;
if( j == system.length() )
break;
count = j + 1;
flag = false;
while( !flag )
{
if( abs(matrix[i][i]) > abs(matrix[j][i]) )
{
z = matrix[i][i] / matrix[j][i];
for( k = i ; k < numberOfVariables ; k++ )
matrix[i][k] = matrix[i][k] - matrix[j][k] * z;
b[i] = b[i] * A.getFPGroup().raiseToPower(b[j],-z);
}
else
{
z = matrix[j][i] / matrix[i][i];
for( k = i ; k < numberOfVariables ; k++ )
matrix[j][k] = matrix[j][k] - matrix[i][k] * z;
b[j] = b[j] * A.getFPGroup().raiseToPower(b[i],-z);
}
if( !matrix[i][i] || !matrix[j][i] )
{
if( matrix[i][i] == 0 )
{
tmp = matrix[i];
matrix[i] = matrix[j];
matrix[j] = tmp;
r = b[i];
b[i] = b[j];
b[j] = r;
}
flag = true;
}
}
if( count == system.length() )
done = true;
}
check = true;
for( j = i + 1 ; j < numberOfVariables ; j++ )
if( matrix[i][j] % matrix[i][i] )
{
check = false;
break;
}
if( check )
break;
done = false;
count = i + 1;
while( !done )
{
for( j = count ; j < numberOfVariables ; j++ )
if( matrix[i][j] && abs(matrix[i][i]) != abs(matrix[i][j]) )
break;
if( j == numberOfVariables )
break;
count = j + 1;
flag = false;
while( !flag )
{
if( abs(matrix[i][i]) > abs(matrix[i][j]) )
{
z = matrix[i][i] / matrix[i][j];
for( k = i ; k < system.length() ; k++ )
matrix[k][i] = matrix[k][i] - matrix[k][j] * z;
trans[0] = j;
trans[1] = i;
trans[2] = -z;
transform.append( trans );
}
else
{
z = matrix[i][j] / matrix[i][i];
for( k = i ; k < system.length() ; k++ )
matrix[k][j] = matrix[k][j] - matrix[k][i] * z;
trans[0] = i;
trans[1] = j;
trans[2] = -z;
transform.append( trans );
}
if( !matrix[i][i] || !matrix[i][j] )
{
if( matrix[i][i] == 0 )
{
for( k = i ; k < system.length() ; k++ )
{
int a = matrix[k][i];
matrix[k][i] = matrix[k][j];
matrix[k][j] = a;
}
if( i != j )
{
trans[0] = i;
trans[1] = j;
trans[2] = 0;
transform.append( trans );
}
}
flag = true;
}
}
if( count == numberOfVariables )
done = true;
}
check = true;
for( j = i + 1 ; j < system.length() ; j++ )
if( matrix[j][i] % matrix[i][i] )
{
check = false;
break;
}
if( check )
break;
}
for( j = i + 1 ; j < system.length() ; j++ )
if( matrix[j][i] )
{
int z = matrix[j][i] / matrix[i][i];
for( k = i ; k < numberOfVariables ; k++ )
matrix[j][k] = matrix[j][k] - matrix[i][k] * z;
b[j] = b[j] * A.getFPGroup().raiseToPower(b[i],-z);
}
for( j = i + 1 ; j < numberOfVariables ; j++ )
if( matrix[i][j] )
{
int z = matrix[i][j] / matrix[i][i];
for( k = i ; k < system.length() ; k++ )
matrix[k][j] = matrix[k][j] - matrix[k][i] * z;
trans[0] = i;
trans[1] = j;
trans[2] = -z;
transform.append( trans );
}
sysRank++;
}
for( i = sysRank ; i < system.length() ; i++ )
{
if( !A.isTrivial( Word( b[i] ) ) )
{
haveSol = -1;
if( out )
if( system.length() > 1 )
file << "while computing the canonical form of the system it was found that this system has no solutions." << endl;
else
file << "while computing the canonical form of the equation it was found that this equation has no solutions." << endl;
return;
}
}
// finding solutions and output in file
if( out )
{
file << "The canonical form: ";
file << endl << endl;
}
for( int p = 0 ; p < sysRank ; p++ )
{
if( out )
{
file << "x" << p + 1;
if( matrix[p][p] != 1 )
file << "^" << matrix[p][p];
file << " = ";
}
AbelianWord w = A.oldInAbelianForm( b[p] );
w = A.oldToNewGens( w );
w = A.newToOldGens( w );
b[p] = w.getWord().freelyReduce();
if( out )
{
A.getFPGroup().printWord( file , b[p] );
file << endl;
}
}
VectorOf<int> xNums( numberOfVariables );
for( i = 0 ; i < sysRank ; i++ )
if( root( b[i] , matrix[i][i] ) )
{
x[i] = b[i];
xNums[i] = i;
if( !A.isFree() )
{
VectorOf<int> tmp(2);
tmp[0] = ( matrix[i][i] > 0 ) ? matrix[i][i] : -matrix[i][i];
tmp[1] = 1;
torsion[i].append( tmp );
}
}
else
{
haveSol = -1;
if( out )
{
if( system.length() > 1 )
file << endl << "The system is unsolvable because there are no solutions for: " << endl << endl;
else
file << endl << "The equation is unsolvable because there are no solutions for: " << endl << endl;
file << "x" << i + 1;
if( matrix[i][i] != 1 )
file << "^" << matrix[i][i];
file << " = ";
A.getFPGroup().printWord( file , b[i].freelyReduce() );
}
return;
}
for( j = sysRank ; j < numberOfVariables ; j++ )
{
x[j] = Word();
xNums[j] = j;
VectorOf<int> tmp(2);
tmp[0] = j - sysRank + 1;
tmp[1] = 1;
params[j].append( tmp );
}
for( i = transform.length() - 1 ; i >= 0 ; i-- )
{
trans = transform[i];
if( !trans[2] )
{
int ind1 = xNums.indexOf( trans[0] );
int ind2 = xNums.indexOf( trans[1] );
xNums[ind1] = trans[1];
xNums[ind2] = trans[0];
}
else
{
int ind1 = xNums.indexOf( trans[0] );
int ind2 = xNums.indexOf( trans[1] );
x[ind1] *= A.getFPGroup().raiseToPower( x[ind2] , trans[2] );
int len = torsion[ind1].length();
for( j = 0 ; j < torsion[ind2].length() ; j++ )
{
bool f = false;
for( k = 0 ; k < len ; k++ )
if( torsion[ind1][k][0] == torsion[ind2][j][0] )
{
f = true;
break;
}
if( f )
torsion[ind1][k][1] += torsion[ind2][j][1] * trans[2];
else
{
VectorOf<int> tmp = torsion[ind2][j];
tmp[1] *= trans[2];
torsion[ind1].append( tmp );
}
}
len = params[ind1].length();
for( j = 0 ; j < params[ind2].length() ; j++ )
{
bool f = false;
for( k = 0 ; k < len ; k++ )
if( params[ind1][k][0] == params[ind2][j][0] )
{
f = true;
break;
}
if( f )
params[ind1][k][1] += params[ind2][j][1] * trans[2];
else
{
VectorOf<int> tmp = params[ind2][j];
tmp[1] *= trans[2];
params[ind1].append( tmp );
}
}
}
}
for( i = 0 ; i < x.length() ; i++ )
{
AbelianWord w = A.oldInAbelianForm( x[i] );
w = A.oldToNewGens( w );
w = A.newToOldGens( w );
x[i] = w.getWord();
}
// output in file
if( out )
{
file << endl << "The set of solutions can be presented as follows: " << endl << endl;
FPGroup G = rawA.getFPGroup();
FPGroup G1 = A.getFPGroup();
for( i = 0 ; i < numberOfVariables ; i++ )
{
int n = xNums.indexOf( i );
bool flag = false;
G.printWord( file , Generator( i + 1 ) );
file << " -> ";
if( x[n].length() )
{
G1.printWord( file , x[n].freelyReduce() );
flag = true;
}
for( j = 0 ; j < torsion[n].length() ; j++ )
{
VectorOf<int> tmp = torsion[n][j];
int z = tmp[1] % tmp[0];
if( z )
if( tmp[1] > 0 )
{
if( flag )
file << " + ";
if( tmp[1] != 1 )
file << tmp[1] << " p( " << tmp[0] << " )";
else
file << "p( " << tmp[0] << " )";
flag = true;
}
else
{
file << " - ";
if( tmp[1] != -1 )
file << -tmp[1] << " p( " << tmp[0] << " )";
else
file << "p( " << tmp[0] << " )";
flag = true;
}
}
for( j = 0 ; j < params[n].length() ; j++ )
{
VectorOf<int> tmp = params[n][j];
if( tmp[1] )
if( tmp[1] > 0 )
{
if( flag )
file << " + ";
if( tmp[1] != 1 )
file << tmp[1] << " t" << tmp[0];
else
file << "t" << tmp[0];
flag = true;
}
else
{
file << " - ";
if( tmp[1] != -1 )
file << -tmp[1] << " t" << tmp[0];
else
file << "t" << tmp[0];
flag = true;
}
}
if( !flag )
file << "1 ";
if( i != numberOfVariables )
file << ",";
file << endl;
}
file << endl << "where p( n ) is any element of n-heigth and t_i - any element of the group." << endl;
}
VectorOf< VectorOf< VectorOf<int> > > t( numberOfVariables );
VectorOf< VectorOf< VectorOf<int> > > p( numberOfVariables );
VectorOf<Word> x1( numberOfVariables );
for( i = 0 ; i < numberOfVariables ; i++ )
{
int n = xNums.indexOf( i );
x1[i] = x[n];
t[i] = torsion[n];
p[i] = params[n];
}
torsion = t;
params = p;
x = x1;
for( i = 0 ; i < system.length() ; i++ )
delete [] matrix[i];
delete [] matrix;
}
bool configure(ResourceFinder &rf)
{
string name=rf.check("name",Value("teleop-icub")).asString().c_str();
string robot=rf.check("robot",Value("icub")).asString().c_str();
string geomagic=rf.check("geomagic",Value("geomagic")).asString().c_str();
double Tp2p=rf.check("Tp2p",Value(1.0)).asDouble();
part=rf.check("part",Value("right_arm")).asString().c_str();
simulator=rf.check("simulator",Value("off")).asString()=="on";
gaze=rf.check("gaze",Value("off")).asString()=="on";
minForce=fabs(rf.check("min-force-feedback",Value(3.0)).asDouble());
maxForce=fabs(rf.check("max-force-feedback",Value(15.0)).asDouble());
bool torso=rf.check("torso",Value("on")).asString()=="on";
Property optGeo("(device hapticdeviceclient)");
optGeo.put("remote",("/"+geomagic).c_str());
optGeo.put("local",("/"+name+"/geomagic").c_str());
if (!drvGeomagic.open(optGeo))
return false;
drvGeomagic.view(igeo);
if (simulator)
{
simPort.open(("/"+name+"/simulator:rpc").c_str());
if (!Network::connect(simPort.getName().c_str(),"/icubSim/world"))
{
yError("iCub simulator is not running!");
drvGeomagic.close();
simPort.close();
return false;
}
}
if (gaze)
{
Property optGaze("(device gazecontrollerclient)");
optGaze.put("remote","/iKinGazeCtrl");
optGaze.put("local",("/"+name+"/gaze").c_str());
if (!drvGaze.open(optGaze))
{
drvGeomagic.close();
simPort.close();
return false;
}
drvGaze.view(igaze);
}
Property optCart("(device cartesiancontrollerclient)");
optCart.put("remote",("/"+robot+"/cartesianController/"+part).c_str());
optCart.put("local",("/"+name+"/cartesianController/"+part).c_str());
if (!drvCart.open(optCart))
{
drvGeomagic.close();
if (simulator)
simPort.close();
if (gaze)
drvGaze.close();
return false;
}
drvCart.view(iarm);
Property optHand("(device remote_controlboard)");
optHand.put("remote",("/"+robot+"/"+part).c_str());
optHand.put("local",("/"+name+"/"+part).c_str());
if (!drvHand.open(optHand))
{
drvGeomagic.close();
if (simulator)
simPort.close();
if (gaze)
drvGaze.close();
drvCart.close();
return false;
}
drvHand.view(imod);
drvHand.view(ipos);
drvHand.view(ivel);
iarm->storeContext(&startup_context);
iarm->restoreContext(0);
Vector dof(10,1.0);
if (!torso)
dof[0]=dof[1]=dof[2]=0.0;
else
dof[1]=0.0;
iarm->setDOF(dof,dof);
iarm->setTrajTime(Tp2p);
Vector accs,poss;
for (int i=0; i<9; i++)
{
joints.push_back(7+i);
modes.push_back(VOCAB_CM_POSITION);
accs.push_back(1e9);
vels.push_back(100.0);
poss.push_back(0.0);
}
poss[0]=20.0;
poss[1]=70.0;
imod->setControlModes(joints.size(),joints.getFirst(),modes.getFirst());
ipos->setRefAccelerations(joints.size(),joints.getFirst(),accs.data());
ipos->setRefSpeeds(joints.size(),joints.getFirst(),vels.data());
ipos->positionMove(joints.size(),joints.getFirst(),poss.data());
joints.clear();
modes.clear();
vels.clear();
for (int i=2; i<9; i++)
{
joints.push_back(7+i);
modes.push_back(VOCAB_CM_VELOCITY);
vels.push_back(40.0);
}
vels[vels.length()-1]=100.0;
s0=s1=idle;
c0=c1=0;
onlyXYZ=true;
stateStr[idle]="idle";
stateStr[triggered]="triggered";
stateStr[running]="running";
Matrix T=zeros(4,4);
T(0,1)=1.0;
T(1,2)=1.0;
T(2,0)=1.0;
T(3,3)=1.0;
igeo->setTransformation(SE3inv(T));
igeo->setCartesianForceMode();
igeo->getMaxFeedback(maxFeedback);
Tsim=zeros(4,4);
Tsim(0,1)=-1.0;
Tsim(1,2)=1.0; Tsim(1,3)=0.5976;
Tsim(2,0)=-1.0; Tsim(2,3)=-0.026;
Tsim(3,3)=1.0;
pos0.resize(3,0.0);
rpy0.resize(3,0.0);
x0.resize(3,0.0);
o0.resize(4,0.0);
if (simulator)
{
Bottle cmd,reply;
cmd.addString("world");
cmd.addString("mk");
cmd.addString("ssph");
// radius
cmd.addDouble(0.02);
// position
cmd.addDouble(0.0);
cmd.addDouble(0.0);
cmd.addDouble(0.0);
// color
cmd.addInt(1);
cmd.addInt(0);
cmd.addInt(0);
// collision
cmd.addString("FALSE");
simPort.write(cmd,reply);
}
forceFbPort.open(("/"+name+"/force-feedback:i").c_str());
feedback.resize(3,0.0);
return true;
}
void checkSendReceiveInt()
{
report(0, "check VectorO<int> send receive");
{
Port portIn;
Port portOut;
portOut.open("/harness_sig/vtest/o");
portIn.open("/harness_sig/vtest/i");
Network::connect("/harness_sig/vtest/o", "/harness_sig/vtest/i");
portOut.enableBackgroundWrite(true);
VectorOf<int> vector;
vector.resize(10);
for (unsigned int k = 0; k < vector.size(); k++)
{
vector[k] = k;
}
bool success = true;
portOut.write(vector);
VectorOf<int> tmp;
portIn.read(tmp);
//compare vector and tmp
if (tmp.size() != vector.size())
{
success = false;
}
else
{
for (unsigned int k = 0; k < vector.size(); k++)
{
if (tmp[k] != vector[k])
success = false;
}
}
checkTrue(success, "VectorOf<int> was sent and received correctly");
portOut.interrupt();
portOut.close();
portIn.interrupt();
portIn.close();
}
report(0, "check VectorOf<int> bottle compatibility");
{
//write the same vector again and receive it as a bottle
Port portIn;
Port portOut;
bool success = true;
portOut.open("/harness_sig/vtest/o");
portIn.open("/harness_sig/vtest/i");
Network::connect("/harness_sig/vtest/o", "/harness_sig/vtest/i");
portOut.enableBackgroundWrite(true);
VectorOf<int> vector;
vector.resize(10);
for (unsigned int k = 0; k < vector.size(); k++)
{
vector[k] = k;
}
portOut.write(vector);
Bottle tmp2;
success = portIn.read(tmp2);
checkTrue(success,"correctly read from the port");
//compare vector and tmp
success = true;
if (tmp2.size() != vector.size())
{
success = false;
}
else
{
for (unsigned int k = 0; k < vector.size(); k++)
{
if (tmp2.get(k).asInt32() != vector[k])
success = false;
}
}
checkTrue(success, "VectorOf<int> was received correctly in a Bottle");
portOut.interrupt();
portOut.close();
portIn.interrupt();
portIn.close();
}
}
main()
{
FPGroup G;
VectorOf<int> order;
cin >> G;
char ch;
cin >> ch;
if (ch != '[') { cerr << "Unexpected input, aborted"<<endl; exit(1);}
do {
int i;
cin >> i;
order.append(i);
cin>> ch;
if (ch==']') break;
else if (ch!=','){ cerr << "Unexpected input, aborted"<<endl; exit(1);}
} while (ch==',');
WordOrder word_order("ShortLex",order);
KBmagPackage kbmag(G.namesOfGenerators(),G.getRelators(),word_order,20);
if ( !kbmag.sanityCheck() ) {
error("kbmag failed sanity check.\n");
}
if (kbmag.autgroup()==yes){
cout << "Group "<< G << " is proved shortlex automatic"<<endl;
GroupDFSA WA = kbmag.wordAcceptor();
WA.setName("Word_acceptor");
WA.printOn();
GenMult GM = kbmag.generalMultiplier();
GM.setName("General_multiplier");
GM.printOn();
DiffMachine D1 = kbmag.differenceMachine(1);
D1.setName("1stDiffMachine");
D1.printOn();
DiffMachine D2 = kbmag.differenceMachine(2);
D2.setName("2ndDiffMachine");
D2.printOn();
}
else
cout << "Group "<< G << " is not proved shortlex automatic"<<endl;
cout << endl;
FPGroup G2;
cin >> G2;
KBmagPackage kbmag2(G2.namesOfGenerators(),G2.getRelators());
if ( !kbmag2.sanityCheck() ) {
error("kbmag2 failed sanity check.\n");
}
if (kbmag2.kbprog()==yes && kbmag2.gpmakefsa()==yes && kbmag2.gpaxioms()==yes){
cout << "Group "<< G2 << " is proved shortlex automatic"<<endl;
GroupDFSA WA = kbmag2.wordAcceptor();
WA.setName("Word_acceptor");
WA.printOn();
GenMult GM = kbmag2.generalMultiplier();
GM.setName("General_multiplier");
GM.printOn();
DiffMachine D1 = kbmag2.differenceMachine(1);
D1.setName("1stDiffMachine");
D1.printOn();
DiffMachine D2 = kbmag2.differenceMachine(2);
D2.setName("2ndDiffMachine");
D2.printOn();
}
else
cout << "Group "<< G2 << " is not proved shortlex automatic"<<endl;
cout << endl;
FPGroup G3;
cin >> G3;
KBmagPackage kbmag3(G3.namesOfGenerators(),G3.getRelators());
if ( !kbmag3.sanityCheck() ) {
error("kbmag3 failed sanity check.\n");
}
if (kbmag3.kbprog()>0){
Bool abort=NO;
int loop=0;
do {
loop++;
cout << "Trying to built automata."<< endl;
if (loop>1) cout << "Pass no. " << loop <<" though loop."<< endl;
if (kbmag3.gpwa()!=yes || kbmag3.gpgenmult()!=yes) abort=YES;
if (abort){ cout << "Failed, giving up!"<< endl; break;}
GroupDFSA WA = kbmag3.wordAcceptor();
WA.setName("Word_acceptor");
WA.printOn();
GenMult GM = kbmag3.generalMultiplier();
GM.setName("GeneralMultiplier");
GM.printOn();
DiffMachine D1 = kbmag3.differenceMachine(1);
D1.setName("1stDiffMachine");
D1.printOn();
DiffMachineRep D2 = kbmag3.differenceMachineRep(2);
D2.setName("2ndDiffMachine");
D2.printOn();
} while (kbmag3.gpcheckmult()==no);
if (abort==NO && kbmag3.gpaxioms()==yes)
cout << "Group "<< G3 << " is proved shortlex automatic"<<endl;
else
cout << "Group "<< G3 << " is not proved shortlex automatic"<<endl;
}
GroupDFSARep WA2 = kbmag3.wordAcceptorRep();
DiffMachineRep D3 = kbmag3.differenceMachineRep(2);
KBmagPackage kbmag4(G3.namesOfGenerators(),G3.getRelators());
kbmag4.setWordAcceptor(WA2);
kbmag4.setDifferenceMachine(D3,2);
kbmag4.gpgenmult();
DFSA F;
F.readFrom();
kbmag4.minimize(F);
F.printOn();
GroupDFSARep F2;
F2.readFrom();
kbmag4.minimize(F2);
F2.printOn();
GroupDFSA M1;
M1.readFrom();
GroupDFSA M2;
M2.readFrom();
GroupDFSA M3;
kbmag4.gpcomp(M1,M2,M3);
M3.printOn();
GroupDFSARep N1;
N1.readFrom();
GroupDFSARep N2;
N2.readFrom();
GroupDFSARep N3;
kbmag4.gpcomp(N1,N2,N3);
N3.printOn();
}
bool imuIdentifierThread::setHeadCtrlModes(const string &_s)
{
printMessage(1,"Setting %s mode for head joints..\n",_s.c_str());
if (_s!="position" && _s!="velocity")
return false;
VectorOf<int> jointsToSet;
jointsToSet.push_back(0);
jointsToSet.push_back(1);
jointsToSet.push_back(2);
VectorOf<int> modes;
if (_s=="position")
{
modes.push_back(VOCAB_CM_POSITION);
modes.push_back(VOCAB_CM_POSITION);
modes.push_back(VOCAB_CM_POSITION);
}
else if (_s=="velocity")
{
modes.push_back(VOCAB_CM_VELOCITY);
modes.push_back(VOCAB_CM_VELOCITY);
modes.push_back(VOCAB_CM_VELOCITY);
}
imodH -> setControlModes(jointsToSet.size(),
jointsToSet.getFirst(),
modes.getFirst());
Time::delay(0.1);
return true;
}
bool reactCtrlThread::setCtrlModes(const VectorOf<int> &jointsToSet,
const string &_p, const string &_s)
{
if (_s!="position" && _s!="velocity")
return false;
if (jointsToSet.size()==0)
return true;
VectorOf<int> modes;
for (size_t i=0; i<jointsToSet.size(); i++)
{
if (_s=="position")
{
modes.push_back(VOCAB_CM_POSITION);
}
else if (_s=="velocity")
{
modes.push_back(VOCAB_CM_VELOCITY);
}
}
if (_p=="arm")
{
imodA->setControlModes(jointsToSet.size(),
jointsToSet.getFirst(),
modes.getFirst());
}
else if (_p=="torso")
{
imodT->setControlModes(jointsToSet.size(),
jointsToSet.getFirst(),
modes.getFirst());
}
else
return false;
return true;
}
bool PresentationParser::getGeneratorRange( const Chars& name,
VectorOf<Chars>& result,
Chars& errMesg )
{
if ( curToken != DOT )
error("bool PresentationParser::getGeneratorRange() "
"can't read the range");
// Make sure, that we have 3 dots in a row
for (int i = 0;i<2;i++){
getToken();
if ( curToken != DOT ) {
parseError("Expected '.' here");
errMesg = parseErrorMessage;
return false;
}
}
getToken();
// Supose to be ','
if ( curToken != COMMA ) {
parseError("Expected ',' here");
errMesg = parseErrorMessage;
return false;
}
getToken();
// Supose to be a generator
if ( curToken != GENERATOR ) {
parseError("Expected a generator here");
errMesg = parseErrorMessage;
return false;
}
Chars name1;
int beginRange;
// Generators must be in the form: <name><index>, where
// index is the first and the last generator index in a range.
//
Chars rangeErrMsg = "When defining a set of generators using a range of "
"subscripts, both the smallest subscript and the largest subscript"
" must be given and the smallest subscript mast be less then the "
"largest one";
if ( !getRangeOf(name,name1,beginRange)) {
parseError(rangeErrMsg);
errMesg = parseErrorMessage;
return false;
}
Chars name2;
int endRange;
if ( !getRangeOf(tokenName,name2,endRange)) {
parseError(rangeErrMsg);
errMesg = parseErrorMessage;
return false;
}
// The last index must be greater than the first one
if ( endRange <= beginRange) {
parseError(rangeErrMsg);
errMesg = parseErrorMessage;
return false;
}
// Names gave to be equal
if ( name2 != name1) {
parseError(rangeErrMsg);
errMesg = parseErrorMessage;
return false;
}
// Generate generators of type: name1<index>, where
// beginRange < index < endRange
int numberOfGens = endRange - beginRange - 1;
if (numberOfGens > 0){
VectorOf<Chars> gens(numberOfGens);
for(int i = 0;i<numberOfGens;i++){
gens[i] = name1+Chars(beginRange + 1 + i);
// Check for duplication and presence of inverses.
if ( result.indexOf(gens[i]) >= 0 ) {
parseError("Duplicate generator");
errMesg = parseErrorMessage;
return false;
} else {
char str[100];
strcpy(str,gens[i]);
invertName(str);
if ( result.indexOf(Chars(str)) >= 0 ) {
parseError("Duplicate generator: formal inverse");
errMesg = parseErrorMessage;
return false;
}
}
}
result = concatenate(result, gens);
}
return true;
}
bool imuIdentifierThread::processWayPoint()
{
processIMU();
if (wayPoints[currentWaypoint].name == "START " ||
wayPoints[currentWaypoint].name == "END " ||
wayPoints[currentWaypoint].name == "MIDDLE " )
{
if (posCtrlFlag)
{
printMessage(1,"Putting head in home position..\n");
goHome();
posCtrlFlag = false;
}
if (yarp::os::Time::now() - timeNow > CTRL_PERIOD)
{
posCtrlFlag = true;
return false;
}
}
else
{
Vector jls = wayPoints[currentWaypoint].jntlims;
Vector vls = wayPoints[currentWaypoint].vels;
bool flag = false;
iencsH->getEncoders(encsH->data());
yarp::sig::Vector head = *encsH;
// ivelH -> velocityMove(vls.data());
VectorOf<int> jointsToSet;
jointsToSet.push_back(0);
jointsToSet.push_back(1);
jointsToSet.push_back(2);
ivelH -> velocityMove(jointsToSet.size(),
jointsToSet.getFirst(),
vls.data());
for (int i = 0; i < 3; i++)
{
if (vls(i) > 0.0)
{
if (jls(i) - head(i) > 0.0)
{
flag = true;
}
}
else if (vls(i) < 0.0)
{
if (jls(i) - head(i) < 0.0)
{
flag = true;
}
}
}
return flag;
}
return true;
}
int main ( )
{
Chars errMsg;
GHNConfig c;
//cout << c << endl;
ifstream in("../wp.in");
in >> c;
cout << c << endl;
int numOfVars;
cout << "Enter the number of variables: ";
cin >> numOfVars;
cout << endl;
FreeGroup G;
cout << "Enter a free group: ";
errMsg = cin >> G;
if( errMsg.length() > 0 ) {
cout << errMsg;
exit(0);
}
cout << endl;
VectorOf<Chars> v = G.namesOfGenerators();
int numOfConst = v.length();
int rLen = v.length() + numOfVars;
VectorOf<Chars> r(rLen);
int j;
for( j = 0; j < v.length(); ++j )
if( v[j][0] != 'x' && v[j][0] != 'X' )
r[j] = v[j];
else
error("x is reserved for variables\n");
char s[3] = {0,0,0};
s[0] = 'x';
for( int i = j; i < rLen; ++i ) {
s[1] = i-j+1+48;
r[i] = s;
}
FreeGroup F(r);
cout << "Enter an equation (a word) with variables x1 ... x"
<< numOfVars << ": ";
Word w;
w = F.readWord(cin,errMsg);
if( errMsg.length() > 0 ) {
cout << errMsg;
exit(0);
}
cout << endl;
VectorOf<Word> im(rLen);
for( int i = 0; i < rLen; ++i )
im[i] = Word(Generator(i+1));
Map M(F,F,im);
int popSize = c.populationSize();
GAWord pop[numOfVars][popSize],newPop[numOfVars][popSize];
for( int k = 0; k < numOfVars; ++k )
for( int i = 0; i < popSize; ++i ) {
pop[k][i] = GAWord(numOfConst,Word());
}
int fit[popSize];
// the main loop
int numOfGens = c.numOfGenerations();
//bool bHaveFitnessScaling = c.haveFitnessScaling();
float crossRate = c.chanceOfCrossover();
float mutRate = c.chanceOfMutation();
UniformRandom devgen;
int max, min, minInd, g;
// create the original random populations
for( int k = 0; k < numOfVars; ++k )
for( int i = 0; i < popSize; ++i ) {
pop[k][i] = pop[k][i].randomWord();
}
for( g = 0; g < numOfGens || numOfGens == -1; ++g ) {
min = MAXINT; max = 0; minInd = -1;
// compute fitness values
for( int i = 0; i < popSize; ++i ) {
for( int k = 0; k < numOfVars; ++k )
M.setGeneratingImages(numOfConst+k,(pop[k][i]).getWord());
fit[i] = M.imageOf(w).freelyReduce().length();
if( fit[i] < min ) {
min = fit[i];
minInd = i;
}
else if( fit[i] > max )
max = fit[i];
}
// print current results
cout << "Generation: " << g << " Fitness: " << min << endl;
/*
if( g % 100 == 0 ) {
for( int i = 0; i < popSize; ++i ) {
cout << "x" << i << " = ";
F.printWord(cout, (pop[i]).getWord());
cout << endl;
}
cout << endl;
}
*/
// exit if found a solution
if( min == 0 ) {
for( int k = 0; k < numOfVars; ++k ) {
cout << "x" << k+1 << " = ";
F.printWord(cout, (pop[k][minInd]).getWord());
cout << endl;
}
return 0;
}
// make fitness values suitable for Roulette wheel selection
int base = max + 1;
for( int i = 0; i < popSize; ++i )
fit[i] = base - fit[i];
// fitness scaling
if( c.haveFitnessScaling() )
for( int i = 0; i < popSize; ++i )
fit[i] = fit[i] * fit[i];
// crossover
RouletteWheel<int> wheel(popSize,fit);
for( int k = 0; k < numOfVars; ++k )
for( int i = 0; i < popSize; ++i ) {
if( devgen.rand() <= crossRate ) {
int i1 = wheel.GetIndex();
int i2 = wheel.GetIndex();
newPop[k][i] = pop[k][i1].crossover(pop[k][i2]);
}
else {
newPop[k][i] = pop[k][i];
}
}
// mutation
for( int k = 0; k < numOfVars; ++k )
for( int i = 0; i < popSize; ++i ) {
if( devgen.rand() <= mutRate ) {
newPop[k][i] = newPop[k][i].mutate();
}
}
// elitist selection
for( int k = 0; k < numOfVars; ++k )
if( c.haveElitistSelection() ) {
newPop[k][0] = pop[k][minInd];
}
// prepare for the next iteration
for( int k = 0; k < numOfVars; ++k )
for( int i = 0; i < popSize; ++i ) {
pop[k][i] = newPop[k][i];
}
}
}