Value RetrieveJointNames::doExecute ()
{
RosJointState& entity = static_cast<RosJointState&> (owner ());
std::vector<Value> values = getParameterValues ();
std::string name = values[0].value ();
if (!dynamicgraph::PoolStorage::getInstance ()->existEntity (name))
{
std::cerr << "invalid entity name" << std::endl;
return Value ();
}
dynamicgraph::sot::Dynamic* dynamic =
dynamic_cast<dynamicgraph::sot::Dynamic*>
(&dynamicgraph::PoolStorage::getInstance ()->getEntity (name));
if (!dynamic)
{
std::cerr << "entity is not a Dynamic entity" << std::endl;
return Value ();
}
CjrlHumanoidDynamicRobot* robot = dynamic->m_HDR;
if (!robot)
{
std::cerr << "no robot in the dynamic entity" << std::endl;
return Value ();
}
entity.jointState ().name.resize (robot->numberDof());
buildJointNames (entity.jointState (), robot->rootJoint());
return Value ();
}
int main(int argc, char *argv[])
{
string aPath;
string aName;
string JointToRank;
string aSpecificitiesFileName;
int InitialPosition = POSITION_STILL;
if (argc!=5)
{
aPath="./";
aName="sample.wrl";
aSpecificitiesFileName = "sampleSpecificities.xml";
JointToRank = "sampleLinkJointRank.xml";
}
else
{
aPath=argv[1];
aName=argv[2];
JointToRank = argv[4];
aSpecificitiesFileName = argv[3];
}
int VerboseMode = 0;
// Read the first humanoid.
CjrlHumanoidDynamicRobot * aHDR = robotDynamicsObjectConstructor.createHumanoidDynamicRobot();
string RobotFileName = aPath+aName;
dynamicsJRLJapan::parseOpenHRPVRMLFile(*aHDR,RobotFileName,JointToRank, aSpecificitiesFileName);
// The second humanoid is constructed through the abstract interface
//
CjrlHumanoidDynamicRobot* a2HDR = robotDynamicsObjectConstructor.createHumanoidDynamicRobot();
// The third humanoid is also constructed through the abstract interface
//
CjrlHumanoidDynamicRobot* a3HDR = robotDynamicsObjectConstructor.createHumanoidDynamicRobot();
// Test the new humanoid structure.
double dInitPos[40] = {
0.0, 0.0, -26.0, 50.0, -24.0, 0.0, 0.0, 0.0, -26.0, 50.0, -24.0, 0.0, // legs
0.0, 0.0, 0.0, 0.0, // chest and head
15.0, -10.0, 0.0, -30.0, 0.0, 0.0, 10.0, // right arm
15.0, 10.0, 0.0, -30.0, 0.0, 0.0, 10.0, // left arm
-20.0, 20.0, -20.0, 20.0, -20.0, // right hand
-10.0, 10.0, -10.0, 10.0, -10.0 // left hand
};
// This is mandatory for this implementation of computeForwardKinematics
// to compute the derivative of the momentum.
{
string inProperty[4]={"TimeStep","ComputeAcceleration",
"ComputeBackwardDynamics", "ComputeZMP"};
string inValue[4]={"0.005","false","false","true"};
for(unsigned int i=0;i<4;i++)
{
aHDR->setProperty(inProperty[i],inValue[i]);
a2HDR->setProperty(inProperty[i],inValue[i]);
a3HDR->setProperty(inProperty[i],inValue[i]);
}
}
int NbOfDofs = aHDR->numberDof();
MAL_VECTOR_DIM(aCurrentConf,double,NbOfDofs);
MAL_VECTOR_DIM(aCurrentVel,double,NbOfDofs);
MAL_VECTOR_FILL(aCurrentVel,0.0);
MAL_VECTOR_DIM(aCurrentAcc,double,NbOfDofs);
MAL_VECTOR_FILL(aCurrentAcc,0.0);
int lindex=0;
for(int i=0;i<6;i++)
aCurrentConf[lindex++] = 0.0;
for(int i=0;i<(NbOfDofs-6 < 40 ? NbOfDofs-6 : 40) ;i++)
{
if (InitialPosition==POSITION_STILL)
aCurrentConf[lindex++] = 0.0;
else if (InitialPosition==POSITION_HALF_SITTING)
aCurrentConf[lindex++] = dInitPos[i]*M_PI/180.0;
}
aHDR->currentConfiguration(aCurrentConf);
aHDR->currentVelocity(aCurrentVel);
aHDR->currentAcceleration(aCurrentAcc);
aHDR->computeForwardKinematics();
HumanoidCopy aHumanoidCopier;
aHumanoidCopier.PerformCopyFromJointsTree(aHDR, a2HDR);
aHumanoidCopier.PerformCopyFromJointsTree(a2HDR, a3HDR);
NbOfDofs = a2HDR->numberDof();
if (VerboseMode>2)
std::cout << "NbOfDofs :" << NbOfDofs << std::endl;
a2HDR->currentConfiguration(aCurrentConf);
a2HDR->currentVelocity(aCurrentVel);
a2HDR->currentAcceleration(aCurrentAcc);
a2HDR->computeForwardKinematics();
a3HDR->currentConfiguration(aCurrentConf);
a3HDR->currentVelocity(aCurrentVel);
a3HDR->currentAcceleration(aCurrentAcc);
a3HDR->computeForwardKinematics();
// Initial Humanoid
ofstream initialhumanoid;
initialhumanoid.open("initialhumanoid.output");
if (initialhumanoid.is_open())
{
DisplayHumanoid(aHDR,initialhumanoid);
initialhumanoid.close();
}
// Copied Humanoid
ofstream copiedhumanoid;
copiedhumanoid.open("copiedhumanoid.output");
if (copiedhumanoid.is_open())
{
DisplayHumanoid(a2HDR,copiedhumanoid);
copiedhumanoid.close();
}
// 2nd copied humanoid
ofstream copied2ndhumanoid;
copied2ndhumanoid.open("copied2ndhumanoid.output");
if (copied2ndhumanoid.is_open())
{
DisplayHumanoid(a3HDR,copied2ndhumanoid);
copied2ndhumanoid.close();
}
MAL_S3_VECTOR(ZMPval,double);
for(int i=0;i<4;i++)
{
aHDR->currentVelocity(aCurrentVel);
aHDR->currentAcceleration(aCurrentAcc);
aHDR->computeForwardKinematics();
ZMPval = aHDR->zeroMomentumPoint();
if (VerboseMode>4)
{
cout << i << "-th value of ZMP : " << ZMPval <<endl;
cout << "Should be equal to the CoM: " << aHDR->positionCenterOfMass() << endl;
}
a2HDR->currentVelocity(aCurrentVel);
a2HDR->currentAcceleration(aCurrentAcc);
a2HDR->computeForwardKinematics();
ZMPval = a2HDR->zeroMomentumPoint();
if(VerboseMode>4)
{
cout << i << "-th value of ZMP : " << ZMPval <<endl;
cout << "Should be equal to the CoM: " << aHDR->positionCenterOfMass() << endl;
}
}
delete aHDR;
delete a2HDR;
delete a3HDR;
string iho("copiedhumanoid.output");
string cho("copied2ndhumanoid.output");
string rho("reportcopy.output");
if (CompareTwoFiles(iho.c_str(),
cho.c_str(),
rho.c_str()))
{
return 0;
}
return -1;
}
int main(int argc, char *argv[])
{
string aSpecificitiesFileName;
string aPath;
string aName;
string aMapFromCjrlJointToRank;
string RefLogFile;
string ActualLogFile;
if (argc!=5)
{
const char *envrobotpath="ROBOTPATH";
char *robotpath = 0;
const char *envrobotname="ROBOT";
char *robotname = 0;
robotpath = getenv(envrobotpath);
robotname = getenv(envrobotname);
if ((robotpath==0) || (robotname==0))
{
cerr << " This program takes 6 arguments: " << endl;
cerr << "./TestHumanoidDynamicRobot PATH_TO_VRML_FILE VRML_FILE_NAME "<< endl;
cerr << " PATH_TO_SPECIFICITIES_XML PATH PATH_TO_MAP_JOINT_2_RANK" << endl;
cerr << " ReferenceLogFile ActualLogFile" << endl;
exit(-1);
}
else
{
aPath=robotpath;
aName=robotname;
aName+="main.wrl";
aSpecificitiesFileName = robotpath;
aSpecificitiesFileName +="/../etc/";
aSpecificitiesFileName += robotname;
aSpecificitiesFileName += "Specificities.xml";
aMapFromCjrlJointToRank = robotpath;
aMapFromCjrlJointToRank += "/../etc/";
aMapFromCjrlJointToRank += robotname;
aMapFromCjrlJointToRank += "JointRank.xml";
}
if (argc==3)
{
RefLogFile = argv[1];
ActualLogFile = argv[2];
}
}
else
{
aPath=argv[1];
aName=argv[2];
aSpecificitiesFileName = argv[3];
aMapFromCjrlJointToRank = argv[4];
}
dynamicsJRLJapan::ObjectFactory dynFactory;
CjrlHumanoidDynamicRobot * aHDR = dynFactory.createHumanoidDynamicRobot();
CjrlHumanoidDynamicRobot * aHDR2 = dynFactory.createHumanoidDynamicRobot();
if (aHDR==0)
{
cerr<< "Dynamic cast on HDR failed " << endl;
exit(-1);
}
cout << "Robot's model file:" << aPath << aName << endl;
cout << "Specificities file:" << aSpecificitiesFileName << endl;
cout << "Map from joint to rank:" << aMapFromCjrlJointToRank << endl;
string RobotFileName = aPath + aName;
parseOpenHRPVRMLFile(*aHDR,RobotFileName,
aMapFromCjrlJointToRank,aSpecificitiesFileName);
parseOpenHRPVRMLFile(*aHDR2,RobotFileName,
aMapFromCjrlJointToRank,aSpecificitiesFileName);
// Display tree of the joints.
int NbOfDofs = aHDR->numberDof();
std::cout << "NbOfDofs :" << NbOfDofs << std::endl;
MAL_VECTOR_DIM(aCurrentConf,double,NbOfDofs);
int lindex=0;
for(int i=0;i<6;i++)
aCurrentConf[lindex++] = 0.0;
for(int i=0;i<(NbOfDofs-6 < 41 ? NbOfDofs-6 : 40) ;i++)
aCurrentConf[lindex++] = 0.0;
aHDR->currentConfiguration(aCurrentConf);
aHDR2->currentConfiguration(aCurrentConf);
const CjrlJoint * LeftFoot = aHDR->leftFoot()->associatedAnkle();
const CjrlJoint * RightFoot = aHDR->rightFoot()->associatedAnkle();
const CjrlJoint * LeftFoot2 = aHDR2->leftFoot()->associatedAnkle();
const CjrlJoint * RightFoot2 = aHDR2->rightFoot()->associatedAnkle();
CjrlJoint * Waist2 = aHDR2->waist();
// Read the data file.
ifstream ActualStateFile;
ActualStateFile.open(ActualLogFile.c_str(),ifstream::in);
if (!ActualStateFile.is_open())
{
cerr << "Unable to open actual state file: " <<
ActualLogFile << endl;
exit(-1);
}
ifstream RefStateFile;
RefStateFile.open(RefLogFile.c_str(),ifstream::in);
if (!RefStateFile.is_open())
{
cerr << "Unable to open reference state file: " <<
RefLogFile << endl;
exit(-1);
}
ofstream RebuildZMP;
RebuildZMP.open("RebuildZMP.dat",ofstream::out);
RebuildZMP.close();
ofstream RebuildForces;
RebuildForces.open("RebuildForces.dat",ofstream::out);
RebuildForces.close();
ofstream RebuildWaist;
RebuildWaist.open("RebuildWaist.dat",ofstream::out);
RebuildWaist.close();
// Set properties for the first model.
{
string inProperty[4]={"TimeStep","ComputeAcceleration",
"ComputeBackwardDynamics", "ComputeZMP"};
string inValue[4]={"0.005","true","true","true"};
for(unsigned int i=0;i<4;i++)
aHDR->setProperty(inProperty[i],inValue[i]);
}
// Set properties for the second model.
{
string inProperty[4]={"TimeStep","ComputeAcceleration",
"ComputeBackwardDynamics", "ComputeZMP"};
string inValue[4]={"0.005","false","false","false"};
for(unsigned int i=0;i<4;i++)
aHDR2->setProperty(inProperty[i],inValue[i]);
}
// Read the first line of actual state:
ofstream ASD("ActualStateDescription.dat");
if (ASD.is_open())
{
for(unsigned int i=0;i<126;i++)
{
string tmp;
ActualStateFile>> tmp;
ASD << i << " : " << tmp << endl;
}
ASD.close();
}
ofstream RSD("ReferenceStateDescription.dat");
if (RSD.is_open())
{
for(unsigned int i=0;i<95;i++)
{
string tmp;
RefStateFile>> tmp;
RSD << i << " : " << tmp << endl;
}
RSD.close();
}
unsigned long int NbIt=0;
double WaistFromRef[6];
double WaistFromActual[6];
matrix4d AbsSupportFootPos;
// We set the first support foot as being the left one.
int PreviousSupportFoot=-1;
while(!ActualStateFile.eof())
{
double NormalForces[2]={0.0,0.0};
double ActualData[131];
double RefData[100];
double RotationFreeFlyer[9];
for(unsigned int i=0;i<131;i++)
{
ActualStateFile >> ActualData[i];
if (i<40)
aCurrentConf[i+6] = ActualData[i];
if (i==82)
NormalForces[1]=ActualData[i];
if (i==88)
NormalForces[0]=ActualData[i];
}
ExtractRefWaist(RefStateFile,WaistFromRef,RotationFreeFlyer,RefData);
for(unsigned int i=0;i<6;i++)
aCurrentConf[i]=WaistFromRef[i];
for(unsigned int i=0;i<40;i++)
aCurrentConf[i+6]=RefData[i];
aHDR2->currentConfiguration(aCurrentConf);
aHDR2->computeForwardKinematics();
ExtractActualWaist(LeftFoot2,
RightFoot2,
Waist2,
AbsSupportFootPos,
WaistFromRef,
RotationFreeFlyer,
NbIt,
WaistFromActual,
PreviousSupportFoot);
SaveWaistPositions(WaistFromRef, WaistFromActual);
if (1)
{
for(unsigned int i=0;i<6;i++)
aCurrentConf[i] = WaistFromActual[i];
}
else
{
for(unsigned int i=0;i<6;i++)
aCurrentConf[i] = WaistFromRef[i];
}
aHDR->currentConfiguration(aCurrentConf);
aHDR->computeForwardKinematics();
vector3d ZMPval;
ZMPval = aHDR->zeroMomentumPoint();
matrix4d TrLF = LeftFoot->currentTransformation();
matrix4d TrRF = RightFoot->currentTransformation();
matrix4d TrLF2 = LeftFoot2->currentTransformation();
matrix4d TrRF2 = RightFoot2->currentTransformation();
double lnorm = NormalForces[0] + NormalForces[1];
RebuildZMP.open("RebuildZMP.dat",ofstream::app);
RebuildZMP << (MAL_S4x4_MATRIX_ACCESS_I_J(TrLF,0,3) * NormalForces[0] +
MAL_S4x4_MATRIX_ACCESS_I_J(TrRF,0,3) * NormalForces[1])/ lnorm << " "
<< (MAL_S4x4_MATRIX_ACCESS_I_J(TrLF,1,3) * NormalForces[0] +
MAL_S4x4_MATRIX_ACCESS_I_J(TrRF,1,3) * NormalForces[1])/ lnorm << " "
<< ZMPval(0) << " " << ZMPval(1) << " "
<< MAL_S4x4_MATRIX_ACCESS_I_J(TrLF,0,3) << " "
<< MAL_S4x4_MATRIX_ACCESS_I_J(TrLF,1,3) << " "
<< MAL_S4x4_MATRIX_ACCESS_I_J(TrLF,2,3) << " "
<< MAL_S4x4_MATRIX_ACCESS_I_J(TrRF,0,3) << " "
<< MAL_S4x4_MATRIX_ACCESS_I_J(TrRF,1,3) << " "
<< MAL_S4x4_MATRIX_ACCESS_I_J(TrRF,2,3) << " "
<< MAL_S4x4_MATRIX_ACCESS_I_J(AbsSupportFootPos,0,3) << " "
<< MAL_S4x4_MATRIX_ACCESS_I_J(AbsSupportFootPos,1,3) << " "
<< MAL_S4x4_MATRIX_ACCESS_I_J(AbsSupportFootPos,2,3) << " "
<< PreviousSupportFoot << " "
<< MAL_S4x4_MATRIX_ACCESS_I_J(TrLF2,0,3) << " "
<< MAL_S4x4_MATRIX_ACCESS_I_J(TrLF2,1,3) << " "
<< MAL_S4x4_MATRIX_ACCESS_I_J(TrLF2,2,3) << " "
<< MAL_S4x4_MATRIX_ACCESS_I_J(TrRF2,0,3) << " "
<< MAL_S4x4_MATRIX_ACCESS_I_J(TrRF2,1,3) << " "
<< MAL_S4x4_MATRIX_ACCESS_I_J(TrRF2,2,3) << endl;
//92
RebuildZMP.close();
NbIt++;
}
ActualStateFile.close();
delete aHDR;
}