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