Example #1
0
 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));
    }
}
Example #3
0
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;
};
Example #4
0
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;
    }
};
Example #5
0
 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;
}
Example #7
0
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);
        };
    }
};
Example #8
0
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));
}
Example #10
0
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;
    }
Example #12
0
 int _getJointAngles (double* ja)
 {
     for ( int i = 0; i < m_robot->numJoints(); i++ ) {
         ja[i] = m_robot->joint(i)->q;
     }
 }
Example #13
0
void convertToBoundingBox(hrp::BodyPtr i_body)
{
    for (int i=0; i<i_body->numLinks(); i++){
        convertToBoundingBox(i_body->link(i));
    }
}
Example #14
0
 inline void calcCMJacobian(const hrp::BodyPtr& body, hrp::Link *base, dmatrix &J) {
   body->calcCMJacobian(base, J);
 }