int main() { KDLCollada kdlCollada; vector <KDL::Chain> kinematicsModels; const string filename = "puma.dae"; // loading collada kinematics model and converting it to kdl serial chain if (!kdlCollada.load(COLLADA_MODELS_PATH + filename, kinematicsModels)) { cout << "Failed to import " << filename; return 0; } cout << "Imported " << kinematicsModels.size() << " kinematics chains" << endl; for (unsigned int i = 0; i < kinematicsModels.size(); i++) // parsing output kdl serail chain { KDL::Chain chain = kinematicsModels[i]; cout << "Chain " << i << " has " << chain.getNrOfSegments() << " segments" << endl; for (unsigned int u = 0; u < chain.getNrOfSegments(); u++) { KDL::Segment segment = chain.segments[u]; string segmentName = segment.getName(); cout << "Segment " << segmentName << " :" <<endl; KDL::Frame f_tip = segment.getFrameToTip(); KDL::Vector rotAxis = f_tip.M.GetRot(); double rotAngle = f_tip.M.GetRotAngle(rotAxis); KDL::Vector trans = f_tip.p; cout << " frame: rotation " << rotAxis.x() << " " << rotAxis.y() << " " << rotAxis.z() << " " << rotAngle * 180 / M_PI << endl; cout << " frame: translation " << trans.x() << " " << trans.y() << " " << trans.z() << endl; KDL::RigidBodyInertia inertia = segment.getInertia(); KDL::Joint joint = segment.getJoint(); string jointName = joint.getName(); string jointType = joint.getTypeName(); KDL::Vector jointAxis = joint.JointAxis(); KDL::Vector jointOrigin = joint.JointOrigin(); cout << " joint name: " << jointName << endl; cout << " type: " << jointType << endl; cout << " axis: " << jointAxis.x() << " " <<jointAxis.y() << " " << jointAxis.z() << endl; cout << " origin: " << jointOrigin.x() << " " << jointOrigin.y() << " " << jointOrigin.z() << endl; } } return 0; }
TEST_F(VectorExpressionGenerationTest, Constructor) { giskard::VectorConstructorSpec descr; giskard::Scope scope; descr.set_x(x); descr.set_y(y); descr.set_z(z); KDL::Vector v = descr.get_expression(scope)->value(); EXPECT_DOUBLE_EQ(v.x(), 1.1); EXPECT_DOUBLE_EQ(v.y(), 2.2); EXPECT_DOUBLE_EQ(v.z(), 3.3); }
// Set vector from tf void set_vector( tf::StampedTransform t, KDL::Vector& v ) { v.x(t.getOrigin().x()); v.y(t.getOrigin().y()); v.z(t.getOrigin().z()); }
// construct vector urdf::Vector3 toUrdf(const KDL::Vector & v) { return urdf::Vector3(v.x(), v.y(), v.z()); }
double leatherman::distance(const KDL::Vector &a, const KDL::Vector &b) { return sqrt((a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z())); }
//KDL vector to eigen void convert(const KDL::Vector &in, eVector3 &out) { out<<in.x(), in.y(), in.z(); }