int _setJointAngles (double* ja) { for ( int i = 0; i < m_robot->numJoints(); i++ ) { m_robot->joint(i)->q = ja[i]; } m_robot->calcForwardKinematics(); return 0; }
void CollisionDetector::setupVClipModel(hrp::BodyPtr i_body) { m_VclipLinks.resize(i_body->numLinks()); std::cerr << i_body->numLinks() << std::endl; for (int i=0; i<i_body->numLinks(); i++) { assert(i_body->link(i)->index == i); setupVClipModel(i_body->link(i)); } }
void hrp::calcAccelerationsForInverseDynamics(const hrp::BodyPtr _m_robot, InvDynStateBuffer& _idsb){ for(int i=0;i<_m_robot->numJoints();i++)_idsb.q(i) = _m_robot->joint(i)->q; _idsb.dq = (_idsb.q - _idsb.q_old) / _idsb.DT; _idsb.ddq = (_idsb.q - 2 * _idsb.q_old + _idsb.q_oldold) / (_idsb.DT * _idsb.DT); const hrp::Vector3 g(0, 0, 9.80665); _idsb.base_p = _m_robot->rootLink()->p; _idsb.base_v = (_idsb.base_p - _idsb.base_p_old) / _idsb.DT; _idsb.base_dv = g + (_idsb.base_p - 2 * _idsb.base_p_old + _idsb.base_p_oldold) / (_idsb.DT * _idsb.DT); _idsb.base_R = _m_robot->rootLink()->R; _idsb.base_dR = (_idsb.base_R - _idsb.base_R_old) / _idsb.DT; _idsb.base_w_hat = _idsb.base_dR * _idsb.base_R.transpose(); _idsb.base_w = hrp::Vector3(_idsb.base_w_hat(2,1), - _idsb.base_w_hat(0,2), _idsb.base_w_hat(1,0)); _idsb.base_dw = (_idsb.base_w - _idsb.base_w_old) / _idsb.DT; };
void hrp::readVirtualForceSensorParamFromProperties (std::map<std::string, hrp::VirtualForceSensorParam>& vfs, hrp::BodyPtr m_robot, const std::string& prop_string, const std::string& instance_name) { coil::vstring virtual_force_sensor = coil::split(prop_string, ","); unsigned int nvforce = virtual_force_sensor.size()/10; for (unsigned int i=0; i<nvforce; i++){ std::string name = virtual_force_sensor[i*10+0]; hrp::dvector tr(7); for (int j = 0; j < 7; j++ ) { coil::stringTo(tr[j], virtual_force_sensor[i*10+3+j].c_str()); } vfs.insert(std::pair<std::string, VirtualForceSensorParam>(name, VirtualForceSensorParam())); VirtualForceSensorParam& p = vfs[name]; p.localPos = hrp::Vector3(tr[0], tr[1], tr[2]); p.localR = Eigen::AngleAxis<double>(tr[6], hrp::Vector3(tr[3],tr[4],tr[5])).toRotationMatrix(); // rotation in VRML is represented by axis + angle p.link = m_robot->link(virtual_force_sensor[i*10+2]); p.id = i; std::cerr << "[" << instance_name << "] virtual force sensor" << std::endl; std::cerr << "[" << instance_name << "] name = " << name << ", parent = " << p.link->name << ", id = " << p.id << std::endl; std::cerr << "[" << instance_name << "] localP = " << p.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[m]" << std::endl; std::cerr << "[" << instance_name << "] localR = " << p.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", " [", "]")) << std::endl; } };
int initializeJointPathExInstance (char* root_link_name, char* target_link_name) { jpe = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(root_link_name), m_robot->link(target_link_name), 0.002, false, std::string("test"))); if ( !jpe ) { std::cerr << print_prefix << " Fail to joint path from " << root_link_name << " to " << target_link_name << std::endl; } else { std::cerr << print_prefix << " Success to joint path from " << root_link_name << " to " << target_link_name << " (dof = " << jpe->numJoints() << std::endl; } return 0; }
bool getJointList(hrp::BodyPtr body, const std::vector<std::string> &elements, std::vector<hrp::Link *> &joints) { if (elements.size() == 0){ for (int i=0; i<body->numJoints(); i++){ joints.push_back(body->joint(i)); } }else{ for (size_t i=0; i<elements.size(); i++){ hrp::Link *j = body->link(elements[i]); if (j){ joints.push_back(j); }else{ std::cerr << "can't find a joint(" << elements[i] << ")" << std::endl; return false; } } } return true; }
void hrp::readInterlockingJointsParamFromProperties (std::vector<std::pair<Link*, Link*> >& pairs, hrp::BodyPtr m_robot, const std::string& prop_string, const std::string& instance_name) { coil::vstring interlocking_joints_str = coil::split(prop_string, ","); size_t ij_prop_num = 2; if (interlocking_joints_str.size() > 0) { size_t num = interlocking_joints_str.size()/ij_prop_num; for (size_t i = 0; i < num; i++) { //std::cerr << "[" << instance_name << "] Interlocking Joints [" << interlocking_joints_str[i*ij_prop_num] << "], [" << interlocking_joints_str[i*ij_prop_num+1] << "]" << std::endl; hrp::Link* link1 = m_robot->link(interlocking_joints_str[i*ij_prop_num]); hrp::Link* link2 = m_robot->link(interlocking_joints_str[i*ij_prop_num+1]); if (link1 == NULL || link2 == NULL) { std::cerr << "[" << instance_name << "] No such interlocking joints [" << interlocking_joints_str[i*ij_prop_num] << "], [" << interlocking_joints_str[i*ij_prop_num+1] << "]" << std::endl; continue; } std::pair<hrp::Link*, hrp::Link*> pair(link1, link2); pairs.push_back(pair); }; } };
void hrp::calcRootLinkWrenchFromInverseDynamics(hrp::BodyPtr _m_robot, InvDynStateBuffer& _idsb, hrp::Vector3& _f_ans, hrp::Vector3& _t_ans){ for(int i=0;i<_m_robot->numJoints();i++){ _m_robot->joint(i)->dq = _idsb.dq(i); _m_robot->joint(i)->ddq = _idsb.ddq(i); } _m_robot->rootLink()->vo = _idsb.base_v - _idsb.base_w.cross(_idsb.base_p); _m_robot->rootLink()->dvo = _idsb.base_dv - _idsb.base_dw.cross(_idsb.base_p) - _idsb.base_w.cross(_idsb.base_v); // calc in differential way _m_robot->rootLink()->w = _idsb.base_w; _m_robot->rootLink()->dw = _idsb.base_dw; _m_robot->calcForwardKinematics(true,true);// calc every link's acc and vel _m_robot->calcInverseDynamics(_m_robot->rootLink(), _f_ans, _t_ans);// this returns f,t at the coordinate origin (not at base link pos) };
void convertToAABB(hrp::BodyPtr i_body) { for (int i=0; i<i_body->numLinks(); i++) convertToAABB(i_body->link(i)); }
void convertToConvexHull(hrp::BodyPtr i_body) { for (int i=0; i<i_body->numLinks(); i++) { convertToConvexHull(i_body->link(i)); } }
RTC::ReturnCode_t setupCollisionModel(hrp::BodyPtr m_robot, const char *url, OpenHRP::BodyInfo_var binfo) { // do qhull OpenHRP::ShapeInfoSequence_var sis = binfo->shapes(); OpenHRP::LinkInfoSequence_var lis = binfo->links(); for(int i = 0; i < m_robot->numLinks(); i++ ) { const OpenHRP::LinkInfo& i_li = lis[i]; const OpenHRP::TransformedShapeIndexSequence& tsis = i_li.shapeIndices; // setup int numTriangles = 0; for (unsigned int l=0; l<tsis.length(); l++) { OpenHRP::ShapeInfo& si = sis[tsis[l].shapeIndex]; const OpenHRP::LongSequence& triangles = si.triangles; numTriangles += triangles.length(); } double points[numTriangles*3]; int points_i = 0; hrp::Matrix44 Rs44; // inv hrp::Matrix33 Rs33 = m_robot->link(i)->Rs; Rs44 << Rs33(0,0),Rs33(0,1), Rs33(0,2), 0, Rs33(1,0),Rs33(1,1), Rs33(1,2), 0, Rs33(2,0),Rs33(2,1), Rs33(2,2), 0, 0.0, 0.0, 0.0, 1.0; for (unsigned int l=0; l<tsis.length(); l++) { const OpenHRP::DblArray12& M = tsis[l].transformMatrix; hrp::Matrix44 T0; T0 << M[0], M[1], M[2], M[3], M[4], M[5], M[6], M[7], M[8], M[9], M[10], M[11], 0.0, 0.0, 0.0, 1.0; hrp::Matrix44 T(Rs44 * T0); const OpenHRP::ShapeInfo& si = sis[tsis[l].shapeIndex]; const OpenHRP::LongSequence& triangles = si.triangles; const float *vertices = si.vertices.get_buffer(); for(unsigned int j=0; j < triangles.length() / 3; ++j){ for(int k=0; k < 3; ++k){ long orgVertexIndex = si.triangles[j * 3 + k]; int p = orgVertexIndex * 3; hrp::Vector4 v(T * hrp::Vector4(vertices[p+0], vertices[p+1], vertices[p+2], 1.0)); points[points_i++] = v[0]; points[points_i++] = v[1]; points[points_i++] = v[2]; } } } hrp::ColdetModelPtr coldetModel(new hrp::ColdetModel()); coldetModel->setName(std::string(i_li.name)); // qhull int vertexIndex = 0; int triangleIndex = 0; int num = 0; char flags[250]; boolT ismalloc = False; sprintf(flags,"qhull Qt Tc"); if (! qh_new_qhull (3,numTriangles,points,ismalloc,flags,NULL,stderr) ) { qh_triangulate(); qh_vertexneighbors(); vertexT *vertex,**vertexp; coldetModel->setNumVertices(qh num_vertices); coldetModel->setNumTriangles(qh num_facets); int index[qh num_vertices]; FORALLvertices { int p = qh_pointid(vertex->point); index[p] = vertexIndex; coldetModel->setVertex(vertexIndex++, points[p*3+0], points[p*3+1], points[p*3+2]); } facetT *facet; num = qh num_facets;; { FORALLfacets { int j = 0, p[3]; setT *vertices = qh_facet3vertex (facet); FOREACHvertexreverse12_ (vertices) { if (j<3) { p[j] = index[qh_pointid(vertex->point)]; } else { fprintf(stderr, "extra vertex %d\n",j); } j++; } coldetModel->setTriangle(triangleIndex++, p[0], p[1], p[2]); } } } // qh_new_qhull coldetModel->build(); m_robot->link(i)->coldetModel = coldetModel; qh_freeqhull(!qh_ALL); int curlong, totlong; qh_memfreeshort (&curlong, &totlong); if (curlong || totlong) { fprintf(stderr, "convhulln: did not free %d bytes of long memory (%d pieces)\n", totlong, curlong); } // std::cerr << std::setw(16) << i_li.name << " reduce triangles from " << std::setw(5) << numTriangles << " to " << std::setw(4) << num << std::endl; }
int _getJointAngles (double* ja) { for ( int i = 0; i < m_robot->numJoints(); i++ ) { ja[i] = m_robot->joint(i)->q; } }
void convertToBoundingBox(hrp::BodyPtr i_body) { for (int i=0; i<i_body->numLinks(); i++){ convertToBoundingBox(i_body->link(i)); } }
inline void calcCMJacobian(const hrp::BodyPtr& body, hrp::Link *base, dmatrix &J) { body->calcCMJacobian(base, J); }