void Foam::RBD::joints::Ryxz::jcalc
(
joint::XSvc& J,
const scalarField& q,
const scalarField& w,
const scalarField& qDot
) const
{
vector qj(q.block<vector>(qIndex_));
scalar s0 = sin(qj.x());
scalar c0 = cos(qj.x());
scalar s1 = sin(qj.y());
scalar c1 = cos(qj.y());
scalar s2 = sin(qj.z());
scalar c2 = cos(qj.z());
J.X.E() = tensor
(
c2*c0 + s2*s1*s0, s2*c1, -c2*s0 + s2*s1*c0,
-s2*c0 + c2*s1*s0, c2*c1, s2*s0 + c2*s1*c0,
c1*s0, -s1, c1*c0
);
J.X.r() = Zero;
J.S = Zero;
J.S.xx() = s2*c1;
J.S.xy() = c2;
J.S.yx() = c2*c1;
J.S.yy() = -s2;
J.S.zx() = -s1;
J.S.zz() = 1;
vector qDotj(qDot.block<vector>(qIndex_));
J.v = J.S & qDotj;
J.c = spatialVector
(
c2*c1*qDotj.z()*qDotj.x()
- s2*s1*qDotj.y()*qDotj.x()
- s2*qDotj.z()*qDotj.y(),
-s2*c1*qDotj.z()*qDotj.x()
- c2*s1*qDotj.y()*qDotj.x()
- c2*qDotj.z()*qDotj.y(),
-c1*qDotj.y()*qDotj.x(),
0,
0,
0
);
}
void
TestSimpleSuccessorArc(const char *name) {
tbb::flow::graph g;
{
REMARK("Join<%s> successor test ", name);
tbb::flow::join_node<tbb::flow::tuple<int>, JNODE_TYPE> qj(g);
tbb::flow::broadcast_node<tbb::flow::tuple<int> > bnode(g);
tbb::flow::make_edge(qj, bnode);
ASSERT(!qj.my_successors.empty(),"successor missing after linking");
g.reset();
ASSERT(!qj.my_successors.empty(),"successor missing after reset()");
g.reset(tbb::flow::rf_clear_edges);
ASSERT(qj.my_successors.empty(), "successors not removed after reset(rf_clear_edges)");
}
}
void
TestSimpleSuccessorArc(const char *name) {
tbb::flow::graph g;
{
//tbb::flow::join_node<tbb::flow::tuple<int>, tbb::flow::queueing> qj(g);
REMARK("Join<%s> successor test ", name);
tbb::flow::join_node<tbb::flow::tuple<int>, JNODE_TYPE> qj(g);
tbb::flow::broadcast_node<tbb::flow::tuple<int> > bnode(g);
tbb::flow::make_edge(qj, bnode);
ASSERT(!qj.my_successors.empty(),"successor missing after linking");
g.reset();
ASSERT(!qj.my_successors.empty(),"successor missing after reset()");
#if TBB_PREVIEW_FLOW_GRAPH_FEATURES
g.reset(tbb::flow::rf_extract);
ASSERT(qj.my_successors.empty(), "successors not removed after reset(rf_extract)");
#endif
}
}
void
TestSimpleSuccessorArc<tbb::flow::tag_matching>(const char *name) {
tbb::flow::graph g;
{
REMARK("Join<%s> successor test ", name);
typedef tbb::flow::tuple<int,int> my_tuple;
tbb::flow::join_node<my_tuple, tbb::flow::tag_matching> qj(g,
tag_func<int>(1),
tag_func<int>(1)
);
tbb::flow::broadcast_node<my_tuple > bnode(g);
tbb::flow::make_edge(qj, bnode);
ASSERT(!qj.my_successors.empty(),"successor missing after linking");
g.reset();
ASSERT(!qj.my_successors.empty(),"successor missing after reset()");
g.reset(tbb::flow::rf_clear_edges);
ASSERT(qj.my_successors.empty(), "successors not removed after reset(rf_clear_edges)");
}
}
int main(int argc, char ** argv)
{
yarp::os::Property prop;
// Parse command line options
prop.fromCommand(argc,argv);
// Skip upper body tests (to support testing iCubHeidelberg01 model)
bool skipUpperBody = prop.check("skipUpperBody");
// Get model name
if( ! prop.find("model").isString() )
{
std::cerr << "icub-model-test error: model option not found" << std::endl;
return EXIT_FAILURE;
}
std::string modelPath = prop.find("model").asString().c_str();
// Open the model
iDynTree::KinDynComputations comp;
bool modelLoaded = comp.loadRobotModelFromFile(modelPath);
if( !modelLoaded )
{
std::cerr << "icub-model-test error: impossible to load model from " << modelLoaded << std::endl;
return EXIT_FAILURE;
}
iDynTree::Vector3 grav;
grav.zero();
iDynTree::JointPosDoubleArray qj(comp.getRobotModel());
iDynTree::JointDOFsDoubleArray dqj(comp.getRobotModel());
qj.zero();
dqj.zero();
comp.setRobotState(qj,dqj,grav);
// Check axis
if( !checkAxisDirections(comp) )
{
return EXIT_FAILURE;
}
// Check if base_link exist, and check that is a frame attached to root_link and if its
// transform is the idyn
if( !checkBaseLink(comp) )
{
return EXIT_FAILURE;
}
// Check if l_sole/r_sole have the same distance from the root_link
if( !checkSolesAreParallel(comp) )
{
return EXIT_FAILURE;
}
// Now some test that test the sensors
iDynTree::ModelLoader mdlLoader;
mdlLoader.loadModelFromFile(modelPath);
if( !checkFTSensorsAreEvenAndNotNull(mdlLoader) )
{
return EXIT_FAILURE;
}
std::cerr << "Check for model " << modelPath << " concluded correctly!" << std::endl;
return EXIT_SUCCESS;
}
