vctFrame4x4<double>
robSAHThumb::ForwardKinematics( const vctFixedSizeVector<double,4>& q,
robSAHThumb::Phalanx phalanx ) const {
switch( phalanx ){
case robSAHThumb::BASE:
return Rtw0;
case robSAHThumb::METACARPUS:
{
vctMatrixRotation3<double> Rb0;
vctFixedSizeVector<double,3> tb0( -3.0/1000.0, 27.1/1000.0, 0.0 );
vctFrame4x4<double> Rtb0(Rb0,tb0);
vctMatrixRotation3<double> Rb1( 0.00000, -1, 0.00000,
0.57358, 0, -0.81915,
0.81915, 0, 0.57358,
VCT_NORMALIZE );
vctFixedSizeVector<double,3> tb1( -9.0/1000.0, 114.0/1000.0, 97.0/1000.0);
vctFrame4x4<double> Rtb1( Rb1, tb1 );
vctFrame4x4<double> Rt0b( Rtb0 );
Rt0b.InverseSelf();
vctFrame4x4<double> Rt01 = Rt0b * Rtb1;
vctMatrixRotation3<double> R( cos( -q[0] ), -sin( -q[0] ), 0.0,
sin( -q[0] ), cos( -q[0] ), 0.0,
0.0, 0.0, 1.0,
VCT_NORMALIZE );
vctFixedSizeVector<double,3> t(0.0);
vctFrame4x4<double> Rt( R, t );
return ForwardKinematics( q, robSAHThumb::BASE ) * Rtb0 * Rt * Rt01;
}
case robSAHThumb::MCP:
{
return ( ForwardKinematics( q, robSAHThumb::METACARPUS ) *
links[0].ForwardKinematics( q[1] ) );
}
case robSAHThumb::PROXIMAL:
return ( ForwardKinematics( q, robSAHThumb::MCP ) *
links[1].ForwardKinematics( q[2] ) );
case robSAHThumb::INTERMEDIATE:
return ( ForwardKinematics( q, robSAHThumb::PROXIMAL ) *
links[2].ForwardKinematics( q[3] ) );
case robSAHThumb::DISTAL:
return ( ForwardKinematics( q, robSAHThumb::INTERMEDIATE ) *
links[3].ForwardKinematics( q[3] ) );
}
}
/* funzione per interpolare l'energia */
Double_t EN ( Double_t *x, Double_t *par ) {
/* variabile ausiliaria: par[0] = $\hbar c$ */
Double_t l = TMath::Power( B * par[0] / x[0], 1./3. );
/*
* termine tra parentesi (ho sfruttato le funzioni note per non
* dover riscrivere un'altra funzione per calcolare $l d\beta/dl$)
*/
Double_t tmp = tb1( l ) - 3. * b1( l );
/* moltiplico per il coefficiente */
tmp = tmp * l * l / ( par[1] * B );
/* primo termine fuori dalle parentesi */
tmp += tb0( l );
/* incremento di uno */
tmp += 1.;
/* 'par[2]' è la densità d'energia $e_0$ */
return par[2] - tmp / ( 3. * B * x[0] * x[0] );
}
void CG_GGVar::import(const String& lib,bool reload)
{
CG_GGVar &tb0(CG_GGVar::current());
if (!reload && !tb0[lib].is_nil())
{
return;
}
int idx = -1;
static bst_set<String> loading;
if(!loading.insert(lib).second)
{
return;
}
try
{
DataPtrT<CallableModule> pmodule(new CallableModule(lib));
CodeGen cgen;
cgen.module = lib;
String libfile="";
arr_1t<String> libpaths=string_split(System::GetEnv("EWSL_LIBPATH","ewsl_lib"),";");
bool is_folder=false;
for(size_t i=0;i<libpaths.size();++i)
{
String path=libpaths[i];
if(path=="") continue;
arr_1t<FileItem> files=FSLocal::current().FindFilesEx(path,lib+".*");
if(files.empty()) continue;
is_folder=files[0].flags.get(FileItem::IS_FOLDER);
libfile=System::AdjustPath(path,true)+files[0].filename;
break;
}
if(libfile=="")
{
libfile="ewsl_lib/"+lib + ".ewsl";
}
if(!is_folder)
{
if (cgen.prepare(libfile, Executor::INDIRECT_FILE))
{
Executor lexer;
lexer.push(cgen.get());
if (lexer.callx(0, 0))
{
for (auto x = cgen.cg_exports.begin(); x != cgen.cg_exports.end(); ++x)
{
pmodule->value[*x] = tb0[lib + "." + (*x)];
}
tb0[lib].kptr(pmodule);
loading.erase(lib);
return;
}
}
}
else
{
String modulepath=System::AdjustPath(libfile,true);
if (cgen.prepare(modulepath+"main.ewsl", Executor::INDIRECT_FILE))
{
Executor lexer;
lexer.push(cgen.get());
if (lexer.callx(0, 0))
{
for (auto x = cgen.cg_exports.begin(); x != cgen.cg_exports.end(); ++x)
{
pmodule->value[*x] = tb0[lib + "." + (*x)];
}
tb0[lib].kptr(pmodule);
loading.erase(lib);
return;
}
}
}
}
catch (...)
{
}
loading.erase(lib);
Exception::XError("unknown module:"+lib);
}
