cmuk::CmuDogKinematics() { _centeredFootIK = true; _debugOutput = false; memcpy(_kc.joint_offsets, default_joint_offsets, sizeof(default_joint_offsets)); memcpy(_kc.joint_limits, default_joint_limits, sizeof(default_joint_limits)); memcpy(_kc.prefer_knee_forward, default_prefer_knee_forward, sizeof(default_prefer_knee_forward)); //_kc.h_body = 0.143; _kc.h_body = 0.120; _kc.l_body = 0.30; _kc.w_body_bottom = 0.0648; _kc.r_foot = 0.01; //_kc.d_hip_body_bottom = 0.060; _kc.d_hip_body_bottom = 0.048; _kc.d_foot_shin = 0.008; _kc.natural_height = -(_kc.joint_offsets[0].z() + _kc.joint_offsets[1].z() + _kc.joint_offsets[2].z() + _kc.joint_offsets[3].z()); _kc.body_cg_offset = default_body_cg; _kc.m_body = 2.240; _kc.m_uleg = 0.120; _kc.m_lleg = 0.070; float m_total = 4*_kc.m_uleg + _kc.m_body; _kc.I_body = inertia_tensor(m_total, _kc.l_body, _kc.w_body_bottom, _kc.h_body); _kc.max_hip_speed = 7.0; _kc.max_knee_speed = 10.0; _kc.skel_body_thickness = 0.005; _kc.skel_uleg_rad = 0.015; _kc.skel_knee_rad = 0.020; _kc.skel_lleg_rad = 0.008; }
void Polyhedra::Initialize(){ if (init) return; bool isRandom = false; //get vertices int N = (int) v.size(); if (N==0) { //generate randomly while ((int) v.size()<4) GenerateRandomGeometry(); N = (int) v.size(); isRandom = true; } //compute convex hull of vertices std::vector<CGALpoint> points; points.resize(v.size()); for(int i=0;i<N;i++) { points[i] = CGALpoint(v[i][0],v[i][1],v[i][2]); } CGAL::convex_hull_3(points.begin(), points.end(), P); //connect triagular facets if possible std::transform(P.facets_begin(), P.facets_end(), P.planes_begin(),Plane_equation()); P = Simplify(P, 1E-9); //modify order of v according to CGAl polyhedron int i = 0; v.clear(); for (Polyhedron::Vertex_iterator vIter = P.vertices_begin(); vIter != P.vertices_end(); ++vIter, i++){ v.push_back(Vector3r(vIter->point().x(),vIter->point().y(),vIter->point().z())); } //list surface triangles for plotting faceTri.clear(); std::transform(P.facets_begin(), P.facets_end(), P.planes_begin(),Plane_equation()); for (Polyhedron::Facet_iterator fIter = P.facets_begin(); fIter != P.facets_end(); fIter++){ Polyhedron::Halfedge_around_facet_circulator hfc0; int n = fIter->facet_degree(); hfc0 = fIter->facet_begin(); int a = std::distance(P.vertices_begin(), hfc0->vertex()); for (int i=2; i<n; i++){ ++hfc0; faceTri.push_back(a); faceTri.push_back(std::distance(P.vertices_begin(), hfc0->vertex())); faceTri.push_back(std::distance(P.vertices_begin(), hfc0->next()->vertex())); } } //compute centroid and volume P_volume_centroid(P, &volume, ¢roid); //check vierd behavior of CGAL in tessalation if(isRandom && volume*1.75<4./3.*3.14*size[0]/2.*size[1]/2.*size[2]/2.) { v.clear(); seed = rand(); Initialize(); } Vector3r translation((-1)*centroid); //set centroid to be [0,0,0] for(int i=0;i<N;i++) { v[i] = v[i]-centroid; } if(isRandom) centroid = Vector3r::Zero(); Vector3r origin(0,0,0); //move and rotate also the CGAL structure Polyhedron Transformation t_trans(1.,0.,0.,translation[0],0.,1.,0.,translation[1],0.,0.,1.,translation[2],1.); std::transform( P.points_begin(), P.points_end(), P.points_begin(), t_trans); //compute inertia Real vtet; Vector3r ctet; Matrix3r Itet1, Itet2; Matrix3r inertia_tensor(Matrix3r::Zero()); for(int i=0; i<(int) faceTri.size(); i+=3){ vtet = std::abs((origin-v[faceTri[i+2]]).dot((v[faceTri[i]]-v[faceTri[i+2]]).cross(v[faceTri[i+1]]-v[faceTri[i+2]]))/6.); ctet = (origin+v[faceTri[i]]+v[faceTri[i+1]]+v[faceTri[i+2]]) / 4.; Itet1 = TetraInertiaTensor(origin-ctet, v[faceTri[i]]-ctet, v[faceTri[i+1]]-ctet, v[faceTri[i+2]]-ctet); ctet = ctet-origin; Itet2<< ctet[1]*ctet[1]+ctet[2]*ctet[2], -ctet[0]*ctet[1], -ctet[0]*ctet[2], -ctet[0]*ctet[1], ctet[0]*ctet[0]+ctet[2]*ctet[2], -ctet[2]*ctet[1], -ctet[0]*ctet[2], -ctet[2]*ctet[1], ctet[1]*ctet[1]+ctet[0]*ctet[0]; inertia_tensor = inertia_tensor + Itet1 + Itet2*vtet; } if(std::abs(inertia_tensor(0,1))+std::abs(inertia_tensor(0,2))+std::abs(inertia_tensor(1,2)) < 1E-13){ // no need to rotate, inertia already diagonal orientation = Quaternionr::Identity(); inertia = Vector3r(inertia_tensor(0,0),inertia_tensor(1,1),inertia_tensor(2,2)); }else{ // calculate eigenvectors of I Vector3r rot; Matrix3r I_rot(Matrix3r::Zero()), I_new(Matrix3r::Zero()); matrixEigenDecomposition(inertia_tensor,I_rot,I_new); // I_rot = eigenvectors of inertia_tensors in columns // I_new = eigenvalues on diagonal // set positove direction of vectors - otherwise reloading does not work Matrix3r sign(Matrix3r::Zero()); Real max_v_signed = I_rot(0,0); Real max_v = std::abs(I_rot(0,0)); if (max_v < std::abs(I_rot(1,0))) {max_v_signed = I_rot(1,0); max_v = std::abs(I_rot(1,0));} if (max_v < std::abs(I_rot(2,0))) {max_v_signed = I_rot(2,0); max_v = std::abs(I_rot(2,0));} sign(0,0) = max_v_signed/max_v; max_v_signed = I_rot(0,1); max_v = std::abs(I_rot(0,1)); if (max_v < std::abs(I_rot(1,1))) {max_v_signed = I_rot(1,1); max_v = std::abs(I_rot(1,1));} if (max_v < std::abs(I_rot(2,1))) {max_v_signed = I_rot(2,1); max_v = std::abs(I_rot(2,1));} sign(1,1) = max_v_signed/max_v; sign(2,2) = 1.; I_rot = I_rot*sign; // force the eigenvectors to be right-hand oriented Vector3r third = (I_rot.col(0)).cross(I_rot.col(1)); I_rot(0,2) = third[0]; I_rot(1,2) = third[1]; I_rot(2,2) = third[2]; inertia = Vector3r(I_new(0,0),I_new(1,1),I_new(2,2)); orientation = Quaternionr(I_rot); //rotate the voronoi cell so that x - is maximal inertia axis and z - is minimal inertia axis //orientation.normalize(); //not needed for(int i=0; i< (int) v.size();i++) { v[i] = orientation.conjugate()*v[i]; } //rotate also the CGAL structure Polyhedron Matrix3r rot_mat = (orientation.conjugate()).toRotationMatrix(); Transformation t_rot(rot_mat(0,0),rot_mat(0,1),rot_mat(0,2),rot_mat(1,0),rot_mat(1,1),rot_mat(1,2),rot_mat(2,0),rot_mat(2,1),rot_mat(2,2),1.); std::transform( P.points_begin(), P.points_end(), P.points_begin(), t_rot); } //initialization done init = 1; }