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; }