void igl::sort_triangles( const Eigen::PlainObjectBase<DerivedV> & V, const Eigen::PlainObjectBase<DerivedF> & F, Eigen::PlainObjectBase<DerivedFF> & FF, Eigen::PlainObjectBase<DerivedI> & I) { using namespace Eigen; using namespace igl; using namespace std; // Put model, projection, and viewport matrices into double arrays Matrix4d MV; Matrix4d P; glGetDoublev(GL_MODELVIEW_MATRIX, MV.data()); glGetDoublev(GL_PROJECTION_MATRIX, P.data()); if(V.cols() == 3) { Matrix<typename DerivedV::Scalar, DerivedV::RowsAtCompileTime,4> hV; hV.resize(V.rows(),4); hV.block(0,0,V.rows(),V.cols()) = V; hV.col(3).setConstant(1); return sort_triangles(hV,F,MV,P,FF,I); }else { return sort_triangles(V,F,MV,P,FF,I); } }
void WingGeom::UpdateDrawObj() { Geom::UpdateDrawObj(); Matrix4d attachMat; Matrix4d relTrans; attachMat = ComposeAttachMatrix(); relTrans = attachMat; relTrans.affineInverse(); relTrans.matMult( m_ModelMatrix.data() ); relTrans.postMult( attachMat.data() ); int nxsec = m_XSecSurf.NumXSec(); m_XSecDrawObj_vec.resize( nxsec, DrawObj() ); //==== Tesselate Surface ====// for ( int i = 0 ; i < nxsec ; i++ ) { m_XSecDrawObj_vec[i].m_PntVec = m_XSecSurf.FindXSec( i )->GetDrawLines( m_TessW(), relTrans ); m_XSecDrawObj_vec[i].m_GeomChanged = true; } m_HighlightXSecDrawObj.m_PntVec = m_XSecSurf.FindXSec( m_ActiveAirfoil )->GetDrawLines( m_TessW(), relTrans ); m_HighlightXSecDrawObj.m_GeomChanged = true; double w = m_XSecSurf.FindXSec( m_ActiveAirfoil )->GetXSecCurve()->GetWidth(); Matrix4d mat; m_XSecSurf.GetBasicTransformation( Z_DIR, X_DIR, XS_SHIFT_MID, false, 1.0, mat ); mat.scale( 1.0/w ); VspCurve crv = m_XSecSurf.FindXSec( m_ActiveAirfoil )->GetUntransformedCurve(); crv.Transform( mat ); vector< vec3d > pts; crv.Tesselate( m_TessW(), pts ); m_CurrentXSecDrawObj.m_PntVec = pts; m_CurrentXSecDrawObj.m_LineWidth = 1.0; m_CurrentXSecDrawObj.m_LineColor = vec3d( 0.0, 0.0, 0.0 ); m_CurrentXSecDrawObj.m_Type = DrawObj::VSP_LINES; m_CurrentXSecDrawObj.m_GeomChanged = true; VspCurve inbd = m_XSecSurf.FindXSec( m_ActiveXSec - 1 )->GetCurve(); inbd.Transform( relTrans ); VspCurve outbd = m_XSecSurf.FindXSec( m_ActiveXSec )->GetCurve(); outbd.Transform( relTrans ); BndBox iBBox, oBBox; inbd.GetBoundingBox( iBBox ); outbd.GetBoundingBox( oBBox ); oBBox.Update( iBBox ); m_HighlightWingSecDrawObj.m_PntVec = oBBox.GetBBoxDrawLines(); }
void Data4Viewer::drawRGBView() { _pKinect->_pRGBCamera->setGLProjectionMatrix( 0.1f,100.f); glMatrixMode ( GL_MODELVIEW ); Eigen::Affine3d tmp; tmp.setIdentity(); Matrix4d mv = btl::utility::setModelViewGLfromPrj(tmp); //mv transform X_m to X_w i.e. model coordinate to world coordinate glLoadMatrixd( mv.data() ); _pKinect->_pRGBCamera->renderCameraInLocal(_pKinect->_pCurrFrame->_gRGB, _pGL.get(),false, NULL, 0.2f, true ); //render in model coordinate //PRINTSTR("drawRGBView"); return; }
void XSecSurf::GetBasicTransformation( int pdir, int wdir, int wshift, bool flip, double w, Matrix4d &mat ) { double *m = mat.data(); for ( int i = 0; i < 16; i++ ) { m[i] = 0; } int prow = pdir; // Principal direction of base curves +Z m[ prow + ( Z_DIR * 4 ) ] = 1; int wrow = wdir; // Width direction of base curves +X m[ wrow + ( X_DIR * 4 ) ] = 1; // Remaining row via clever math int row = 3 - ( prow + wrow ); // Cross product to ensure right handed system int r1, r2; switch( row ) { case X_DIR: r1 = Y_DIR; r2 = Z_DIR; break; case Y_DIR: r1 = Z_DIR; r2 = X_DIR; break; case Z_DIR: r1 = X_DIR; r2 = Y_DIR; break; } int flipflag = 1; if ( flip ) { flipflag = -1; } // Specialized cross product with known zeros. m[ row + ( Y_DIR * 4 ) ] = flipflag * ( m[ r1 + ( Z_DIR * 4 ) ] * m[ r2 + ( X_DIR * 4 ) ] - m[ r1 + ( X_DIR * 4 ) ] * m[ r2 + ( Z_DIR * 4 ) ] ); // Shift in width direction if required. m[ wrow + 12 ] = -w * wshift / 2.0; }
//==== Transform Control Points =====// void VspCurve::Transform( Matrix4d & mat ) { curve_rotation_matrix_type rmat; double *mmat( mat.data() ); curve_point_type trans; rmat << mmat[0], mmat[4], mmat[8], mmat[1], mmat[5], mmat[9], mmat[2], mmat[6], mmat[10]; trans << mmat[12], mmat[13], mmat[14]; m_Curve.rotate( rmat ); m_Curve.translate( trans ); }
void CData4Viewer::drawDepthView(qglviewer::Camera* pCamera_) { _pKinect->_pRGBCamera->setGLProjectionMatrix( 0.01f,20.f); _pDepth->setRTw( Matrix3f::Identity(),Vector3f(0,0,0) ); //_pDepth->gpuTransformToWorldCVCV(); Matrix4f mModelView; _pDepth->getGLModelViewMatrix(&mModelView); Matrix4d mTmp = mModelView.cast<double>(); pCamera_->setFromModelViewMatrix( mTmp.data() ); _pDepth->gpuRender3DPts(_pGL.get(), _pGL->_usLevel); //PRINTSTR("drawDepthView"); return; }
void Data4Viewer::drawDepthView(qglviewer::Camera* pCamera_) { _pKinect->_pRGBCamera->setGLProjectionMatrix( 0.01f,20.f); //set camera parameters Matrix4d mModelView = btl::utility::setModelViewGLfromRTCV(SO3Group<double>(), Vector3d(0, 0, 0)); Matrix4d mTmp = mModelView.cast<double>(); pCamera_->setFromModelViewMatrix( mTmp.data() ); //display but rendering into buffers glPointSize(3.f); glColor3f(1.f, 1.f, 1.f); if (_Pts.empty()) return; _pGL->gpuMapPtResources(_Pts); _pGL->gpuMapNlResources(_Nls); glDrawArrays(GL_POINTS, 0, btl::kinect::__aDepthWxH[2]); glDisableClientState(GL_VERTEX_ARRAY); glDisableClientState(GL_NORMAL_ARRAY); glBindBuffer(GL_ARRAY_BUFFER, 0);// it's crucially important for program correctness, it return the buffer to opengl rendering system. return; }
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] ) { //DEBUG //cout << "constructModelmex: START" << endl; //END_DEBUG char buf[100]; mxArray *pm; if (nrhs!=1) { mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","Usage model_ptr = constructModelmex(obj)"); } if (isa(prhs[0],"DrakeMexPointer")) { // then it's calling the destructor destroyDrakeMexPointer<RigidBodyManipulator*>(prhs[0]); return; } const mxArray* pRBM = prhs[0]; RigidBodyManipulator *model=NULL; // model->robot_name = get_strings(mxGetProperty(pRBM,0,"name")); const mxArray* pBodies = mxGetProperty(pRBM,0,"body"); if (!pBodies) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","the body array is invalid"); int num_bodies = static_cast<int>(mxGetNumberOfElements(pBodies)); const mxArray* pFrames = mxGetProperty(pRBM,0,"frame"); if (!pFrames) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","the frame array is invalid"); int num_frames = static_cast<int>(mxGetNumberOfElements(pFrames)); pm = mxGetProperty(pRBM, 0, "num_positions"); if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","model should have a num_positions field"); int num_positions = static_cast<int>(*mxGetPrSafe(pm)); model = new RigidBodyManipulator(num_positions, num_bodies, num_frames); for (int i=0; i<model->num_bodies; i++) { //DEBUG //cout << "constructModelmex: body " << i << endl; //END_DEBUG model->bodies[i]->body_index = i; pm = mxGetProperty(pBodies,i,"linkname"); mxGetString(pm,buf,100); model->bodies[i]->linkname.assign(buf,strlen(buf)); pm = mxGetProperty(pBodies,i,"robotnum"); model->bodies[i]->robotnum = (int) mxGetScalar(pm)-1; pm = mxGetProperty(pBodies,i,"mass"); model->bodies[i]->mass = mxGetScalar(pm); pm = mxGetProperty(pBodies,i,"com"); if (!mxIsEmpty(pm)) memcpy(model->bodies[i]->com.data(),mxGetPrSafe(pm),sizeof(double)*3); pm = mxGetProperty(pBodies,i,"I"); if (!mxIsEmpty(pm)) memcpy(model->bodies[i]->I.data(),mxGetPrSafe(pm),sizeof(double)*6*6); pm = mxGetProperty(pBodies,i,"position_num"); model->bodies[i]->position_num_start = (int) mxGetScalar(pm) - 1; //zero-indexed pm = mxGetProperty(pBodies,i,"velocity_num"); model->bodies[i]->velocity_num_start = (int) mxGetScalar(pm) - 1; //zero-indexed pm = mxGetProperty(pBodies,i,"parent"); if (!pm || mxIsEmpty(pm)) model->bodies[i]->parent = nullptr; else { int parent_ind = static_cast<int>(mxGetScalar(pm))-1; if (parent_ind >= static_cast<int>(model->bodies.size())) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","bad body.parent %d (only have %d bodies)",parent_ind,model->bodies.size()); if (parent_ind>=0) model->bodies[i]->parent = model->bodies[parent_ind]; } { mxGetString(mxGetProperty(pBodies, i, "jointname"), buf, 100); string jointname; jointname.assign(buf, strlen(buf)); pm = mxGetProperty(pBodies, i, "Ttree"); // todo: check that the size is 4x4 Isometry3d Ttree; memcpy(Ttree.data(), mxGetPrSafe(pm), sizeof(double) * 4 * 4); int floating = (int) mxGetScalar(mxGetProperty(pBodies, i, "floating")); Eigen::Vector3d joint_axis; pm = mxGetProperty(pBodies, i, "joint_axis"); memcpy(joint_axis.data(), mxGetPrSafe(pm), sizeof(double) * 3); double pitch = mxGetScalar(mxGetProperty(pBodies, i, "pitch")); if (model->bodies[i]->hasParent()) { unique_ptr<DrakeJoint> joint = createJoint(jointname, Ttree, floating, joint_axis, pitch); // mexPrintf((model->bodies[i]->getJoint().getName() + ": " + std::to_string(model->bodies[i]->getJoint().getNumVelocities()) + "\n").c_str()); FixedAxisOneDoFJoint* fixed_axis_one_dof_joint = dynamic_cast<FixedAxisOneDoFJoint*>(joint.get()); if (fixed_axis_one_dof_joint != nullptr) { double joint_limit_min = mxGetScalar(mxGetProperty(pBodies,i,"joint_limit_min")); double joint_limit_max = mxGetScalar(mxGetProperty(pBodies,i,"joint_limit_max")); fixed_axis_one_dof_joint->setJointLimits(joint_limit_min,joint_limit_max); double damping = mxGetScalar(mxGetProperty(pBodies, i, "damping")); double coulomb_friction = mxGetScalar(mxGetProperty(pBodies, i, "coulomb_friction")); double coulomb_window = mxGetScalar(mxGetProperty(pBodies, i, "coulomb_window")); fixed_axis_one_dof_joint->setDynamics(damping, coulomb_friction, coulomb_window); } model->bodies[i]->setJoint(move(joint)); } // pm = mxGetProperty(pBodies,i,"T_body_to_joint"); // memcpy(model->bodies[i]->T_body_to_joint.data(),mxGetPrSafe(pm),sizeof(double)*4*4); } //DEBUG //cout << "constructModelmex: About to parse collision geometry" << endl; //END_DEBUG pm = mxGetProperty(pBodies,i,"collision_geometry"); Matrix4d T; if (!mxIsEmpty(pm)){ for (int j=0; j<mxGetNumberOfElements(pm); j++) { //DEBUG //cout << "constructModelmex: Body " << i << ", Element " << j << endl; //END_DEBUG mxArray* pShape = mxGetCell(pm,j); char* group_name_cstr = mxArrayToString(mxGetProperty(pShape,0,"name")); string group_name; if (group_name_cstr) { group_name = group_name_cstr; } else { group_name = "default"; } // Get element-to-link transform from MATLAB object memcpy(T.data(), mxGetPrSafe(mxGetProperty(pShape,0,"T")), sizeof(double)*4*4); auto shape = (DrakeShapes::Shape)static_cast<int>(mxGetScalar(mxGetProperty(pShape,0,"drake_shape_id"))); vector<double> params_vec; RigidBody::CollisionElement element(T, model->bodies[i]); switch (shape) { case DrakeShapes::BOX: { double* params = mxGetPrSafe(mxGetProperty(pShape,0,"size")); element.setGeometry(DrakeShapes::Box(Vector3d(params[0],params[1],params[2]))); } break; case DrakeShapes::SPHERE: { double r(*mxGetPrSafe(mxGetProperty(pShape,0,"radius"))); element.setGeometry(DrakeShapes::Sphere(r)); } break; case DrakeShapes::CYLINDER: { double r(*mxGetPrSafe(mxGetProperty(pShape,0,"radius"))); double l(*mxGetPrSafe(mxGetProperty(pShape,0,"len"))); element.setGeometry(DrakeShapes::Cylinder(r, l)); } break; case DrakeShapes::MESH: { string filename(mxArrayToString(mxGetProperty(pShape,0,"filename"))); element.setGeometry(DrakeShapes::Mesh(filename, filename)); } break; case DrakeShapes::MESH_POINTS: { mxArray* pPoints; mexCallMATLAB(1,&pPoints,1,&pShape,"getPoints"); int n_pts = static_cast<int>(mxGetN(pPoints)); Map<Matrix3Xd> pts(mxGetPrSafe(pPoints),3,n_pts); element.setGeometry(DrakeShapes::MeshPoints(pts)); mxDestroyArray(pPoints); // The element-to-link transform is applied in // RigidBodyMesh/getPoints - don't apply it again! T = Matrix4d::Identity(); } break; case DrakeShapes::CAPSULE: { double r(*mxGetPrSafe(mxGetProperty(pShape,0,"radius"))); double l(*mxGetPrSafe(mxGetProperty(pShape,0,"len"))); element.setGeometry(DrakeShapes::Capsule(r, l)); } break; default: // intentionally do nothing.. //DEBUG //cout << "constructModelmex: SHOULD NOT GET HERE" << endl; //END_DEBUG break; } //DEBUG //cout << "constructModelmex: geometry = " << geometry.get() << endl; //END_DEBUG model->addCollisionElement(element, model->bodies[i], group_name); } if (!model->bodies[i]->hasParent()) { model->updateCollisionElements(model->bodies[i]); // update static objects only once - right here on load } // Set collision filtering bitmasks pm = mxGetProperty(pBodies,i,"collision_filter"); DrakeCollision::bitmask group, mask; mxArray* belongs_to = mxGetField(pm,0,"belongs_to"); mxArray* ignores = mxGetField(pm,0,"ignores"); if (!(mxIsLogical(belongs_to)) || !isMxArrayVector(belongs_to)) { cout << "is logical: " << mxIsLogical(belongs_to) << endl; cout << "number of dimensions: " << mxGetNumberOfDimensions(belongs_to) << endl; mexErrMsgIdAndTxt("Drake:constructModelmex:BadCollisionFilterStruct", "The 'belongs_to' field of the 'collision_filter' " "struct must be a logical vector."); } if (!(mxIsLogical(ignores)) || !isMxArrayVector(ignores)) { mexErrMsgIdAndTxt("Drake:constructModelmex:BadCollisionFilterStruct", "The 'ignores' field of the 'collision_filter' " "struct must be a logical vector."); } size_t numel_belongs_to(mxGetNumberOfElements(belongs_to)); size_t numel_ignores(mxGetNumberOfElements(ignores)); size_t num_collision_filter_groups = max(numel_belongs_to, numel_ignores); if (num_collision_filter_groups > MAX_NUM_COLLISION_FILTER_GROUPS) { mexErrMsgIdAndTxt("Drake:constructModelmex:TooManyCollisionFilterGroups", "The total number of collision filter groups (%d) " "exceeds the maximum allowed number (%d)", num_collision_filter_groups, MAX_NUM_COLLISION_FILTER_GROUPS); } mxLogical* logical_belongs_to = mxGetLogicals(belongs_to); for (int j = 0; j < numel_belongs_to; ++j) { if (static_cast<bool>(logical_belongs_to[j])) { group.set(j); } } mxLogical* logical_ignores = mxGetLogicals(ignores); for (int j = 0; j < numel_ignores; ++j) { if (static_cast<bool>(logical_ignores[j])) { mask.set(j); } } model->bodies[i]->setCollisionFilter(group,mask); } } // THIS IS UGLY: I'm sending the terrain contact points into the // contact_pts field of the cpp RigidBody objects //DEBUG //cout << "constructModelmex: Parsing contact points " << endl; //cout << "constructModelmex: Get struct" << endl; //END_DEBUG mxArray* contact_pts_struct[1]; if (~mexCallMATLAB(1,contact_pts_struct,1,const_cast<mxArray**>(&pRBM),"getTerrainContactPoints")) { //DEBUG //cout << "constructModelmex: Got struct" << endl; //if (contact_pts_struct) { //cout << "constructModelmex: Struct pointer: " << contact_pts_struct << endl; //} else { //cout << "constructModelmex: Struct pointer NULL" << endl; //} //cout << "constructModelmex: Get numel of struct" << endl; //END_DEBUG const int n_bodies_w_contact_pts = static_cast<int>(mxGetNumberOfElements(contact_pts_struct[0])); //DEBUG //cout << "constructModelmex: Got numel of struct:" << n_bodies_w_contact_pts << endl; //END_DEBUG mxArray* pPts; int body_idx; int n_pts; for (int j=0; j < n_bodies_w_contact_pts; j++) { //DEBUG //cout << "constructModelmex: Loop: Iteration " << j << endl; //cout << "constructModelmex: Get body_idx" << endl; //END_DEBUG body_idx = (int) mxGetScalar(mxGetField(contact_pts_struct[0],j,"idx")) - 1; //DEBUG //cout << "constructModelmex: Got body_idx: " << body_idx << endl; //cout << "constructModelmex: Get points" << endl; //END_DEBUG pPts = mxGetField(contact_pts_struct[0],j,"pts"); //DEBUG //cout << "constructModelmex: Get points" << endl; //cout << "constructModelmex: Get number of points" << endl; //END_DEBUG n_pts = static_cast<int>(mxGetN(pPts)); //DEBUG //cout << "constructModelmex: Got number of points: " << n_pts << endl; //cout << "constructModelmex: Set contact_pts of body" << endl; //END_DEBUG Map<Matrix3Xd> pts(mxGetPrSafe(pPts),3,n_pts); model->bodies[body_idx]->contact_pts = pts; //DEBUG //cout << "constructModelmex: Contact_pts of body: " << endl; //cout << model->bodies[body_idx]->contact_pts << endl; //END_DEBUG } } for (int i=0; i<model->num_frames; i++) { pm = mxGetProperty(pFrames,i,"name"); mxGetString(pm,buf,100); model->frames[i].name.assign(buf,strlen(buf)); pm = mxGetProperty(pFrames,i,"body_ind"); model->frames[i].body_ind = (int) mxGetScalar(pm)-1; pm = mxGetProperty(pFrames,i,"T"); memcpy(model->frames[i].Ttree.data(),mxGetPrSafe(pm),sizeof(double)*4*4); } const mxArray* a_grav_array = mxGetProperty(pRBM,0,"gravity"); if (a_grav_array && mxGetNumberOfElements(a_grav_array)==3) { double* p = mxGetPrSafe(a_grav_array); model->a_grav[3] = p[0]; model->a_grav[4] = p[1]; model->a_grav[5] = p[2]; } else { mexErrMsgIdAndTxt("Drake:constructModelmex:BadGravity","Couldn't find a 3 element gravity vector in the object."); } // LOOP CONSTRAINTS const mxArray* pLoops = mxGetProperty(pRBM,0,"loop"); int num_loops = static_cast<int>(mxGetNumberOfElements(pLoops)); model->loops.clear(); for (int i=0; i<num_loops; i++) { pm = mxGetProperty(pLoops,i,"body1"); int body_A_ind = static_cast<int>(mxGetScalar(pm)-1); pm = mxGetProperty(pLoops,i,"body2"); int body_B_ind = static_cast<int>(mxGetScalar(pm)-1); pm = mxGetProperty(pLoops,i,"pt1"); Vector3d pA; memcpy(pA.data(), mxGetPrSafe(pm), 3*sizeof(double)); pm = mxGetProperty(pLoops,i,"pt2"); Vector3d pB; memcpy(pB.data(), mxGetPrSafe(pm), 3*sizeof(double)); model->loops.push_back(RigidBodyLoop(model->bodies[body_A_ind], pA, model->bodies[body_B_ind], pB)); } //ACTUATORS const mxArray* pActuators = mxGetProperty(pRBM,0,"actuator"); int num_actuators = static_cast<int>(mxGetNumberOfElements(pActuators)); model->actuators.clear(); for (int i=0; i<num_actuators; i++) { pm = mxGetProperty(pActuators,i,"name"); mxGetString(pm,buf,100); pm = mxGetProperty(pActuators,i,"joint"); int joint = static_cast<int>(mxGetScalar(pm)-1); pm = mxGetProperty(pActuators,i, "reduction"); model->actuators.push_back(RigidBodyActuator(std::string(buf), model->bodies[joint], static_cast<double>(mxGetScalar(pm)))); } model->compile(); plhs[0] = createDrakeMexPointer((void*)model,"RigidBodyManipulator"); //DEBUG //cout << "constructModelmex: END" << endl; //END_DEBUG }
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] ) { //DEBUG //cout << "constructModelmex: START" << endl; //END_DEBUG char buf[100]; mxArray *pm; if (nrhs!=1) { mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","Usage model_ptr = constructModelmex(obj)"); } if (isa(prhs[0],"DrakeMexPointer")) { // then it's calling the destructor destroyDrakeMexPointer<RigidBodyManipulator*>(prhs[0]); return; } const mxArray* pRBM = prhs[0]; RigidBodyManipulator *model=NULL; // model->robot_name = get_strings(mxGetProperty(pRBM,0,"name")); const mxArray* featherstone = mxGetProperty(pRBM,0,"featherstone"); if (!featherstone) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs", "the featherstone array is invalid"); const mxArray* pBodies = mxGetProperty(pRBM,0,"body"); if (!pBodies) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","the body array is invalid"); int num_bodies = mxGetNumberOfElements(pBodies); const mxArray* pFrames = mxGetProperty(pRBM,0,"frame"); if (!pFrames) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","the frame array is invalid"); int num_frames = mxGetNumberOfElements(pFrames); // set up the model pm = mxGetField(featherstone,0,"NB"); if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.NB. Are you passing in the correct structure?"); model = new RigidBodyManipulator((int) mxGetScalar(pm), (int) mxGetScalar(pm), num_bodies, num_frames); pm = mxGetField(featherstone,0,"parent"); if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.parent."); double* parent = mxGetPr(pm); pm = mxGetField(featherstone,0,"pitch"); if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.pitch."); double* pitch = mxGetPr(pm); pm = mxGetField(featherstone,0,"dofnum"); if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.dofnum."); double* dofnum = mxGetPr(pm); pm = mxGetField(featherstone,0,"damping"); if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.damping."); memcpy(model->damping.data(),mxGetPr(pm),sizeof(double)*model->NB); pm = mxGetField(featherstone,0,"coulomb_friction"); if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.coulomb_friction."); memcpy(model->coulomb_friction.data(),mxGetPr(pm),sizeof(double)*model->NB); pm = mxGetField(featherstone,0,"static_friction"); if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.static_friction."); memcpy(model->static_friction.data(),mxGetPr(pm),sizeof(double)*model->NB); pm = mxGetField(featherstone,0,"coulomb_window"); if (!pm) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.coulomb_window."); memcpy(model->coulomb_window.data(),mxGetPr(pm),sizeof(double)*model->NB); mxArray* pXtree = mxGetField(featherstone,0,"Xtree"); if (!pXtree) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.Xtree."); mxArray* pI = mxGetField(featherstone,0,"I"); if (!pI) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't find field model.featherstone.I."); for (int i=0; i<model->NB; i++) { model->parent[i] = ((int) parent[i]) - 1; // since it will be used as a C index model->pitch[i] = (int) pitch[i]; model->dofnum[i] = (int) dofnum[i] - 1; // zero-indexed mxArray* pXtreei = mxGetCell(pXtree,i); if (!pXtreei) mexErrMsgIdAndTxt("Drake:HandCpmex:BadInputs","can't access model.featherstone.Xtree{%d}",i); // todo: check that the size is 6x6 memcpy(model->Xtree[i].data(),mxGetPr(pXtreei),sizeof(double)*6*6); mxArray* pIi = mxGetCell(pI,i); if (!pIi) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","can't access model.featherstone.I{%d}",i); // todo: check that the size is 6x6 memcpy(model->I[i].data(),mxGetPr(pIi),sizeof(double)*6*6); } for (int i=0; i<model->num_bodies; i++) { //DEBUG //cout << "constructModelmex: body " << i << endl; //END_DEBUG pm = mxGetProperty(pBodies,i,"linkname"); mxGetString(pm,buf,100); model->bodies[i].linkname.assign(buf,strlen(buf)); pm = mxGetProperty(pBodies,i,"jointname"); mxGetString(pm,buf,100); model->bodies[i].jointname.assign(buf,strlen(buf)); pm = mxGetProperty(pBodies,i,"robotnum"); model->bodies[i].robotnum = (int) mxGetScalar(pm)-1; pm = mxGetProperty(pBodies,i,"mass"); model->bodies[i].mass = (double) mxGetScalar(pm); pm = mxGetProperty(pBodies,i,"com"); if (!mxIsEmpty(pm)) memcpy(model->bodies[i].com.data(),mxGetPr(pm),sizeof(double)*3); pm = mxGetProperty(pBodies,i,"dofnum"); model->bodies[i].dofnum = (int) mxGetScalar(pm) - 1; //zero-indexed pm = mxGetProperty(pBodies,i,"floating"); model->bodies[i].floating = (int) mxGetScalar(pm); pm = mxGetProperty(pBodies,i,"pitch"); model->bodies[i].pitch = (int) mxGetScalar(pm); pm = mxGetProperty(pBodies,i,"parent"); if (!pm || mxIsEmpty(pm)) model->bodies[i].parent = -1; else model->bodies[i].parent = mxGetScalar(pm) - 1; if (model->bodies[i].dofnum>=0) { pm = mxGetProperty(pBodies,i,"joint_limit_min"); model->joint_limit_min[model->bodies[i].dofnum] = mxGetScalar(pm); pm = mxGetProperty(pBodies,i,"joint_limit_max"); model->joint_limit_max[model->bodies[i].dofnum] = mxGetScalar(pm); } pm = mxGetProperty(pBodies,i,"Ttree"); // todo: check that the size is 4x4 memcpy(model->bodies[i].Ttree.data(),mxGetPr(pm),sizeof(double)*4*4); pm = mxGetProperty(pBodies,i,"T_body_to_joint"); memcpy(model->bodies[i].T_body_to_joint.data(),mxGetPr(pm),sizeof(double)*4*4); //DEBUG //cout << "constructModelmex: About to parse collision geometry" << endl; //END_DEBUG pm = mxGetProperty(pBodies,i,"contact_shapes"); Matrix4d T; if (!mxIsEmpty(pm)){ for (int j=0; j<mxGetNumberOfElements(pm); j++) { //DEBUG //cout << "constructModelmex: Body " << i << ", Element " << j << endl; //END_DEBUG mxArray* pShape = mxGetCell(pm,j); char* group_name_cstr = mxArrayToString(mxGetProperty(pShape,0,"name")); string group_name; if (group_name_cstr) { group_name = group_name_cstr; } else { group_name = "default"; } // Get element-to-link transform from MATLAB object memcpy(T.data(), mxGetPr(mxGetProperty(pShape,0,"T")), sizeof(double)*4*4); auto shape = (DrakeCollision::Shape)mxGetScalar(mxGetProperty(pShape,0,"bullet_shape_id")); vector<double> params_vec; switch (shape) { case DrakeCollision::BOX: { double* params = mxGetPr(mxGetProperty(pShape,0,"size")); params_vec.push_back(params[0]); params_vec.push_back(params[1]); params_vec.push_back(params[2]); } break; case DrakeCollision::SPHERE: { params_vec.push_back(*mxGetPr(mxGetProperty(pShape,0,"radius"))); } break; case DrakeCollision::CYLINDER: { params_vec.push_back(*mxGetPr(mxGetProperty(pShape,0,"radius"))); params_vec.push_back(*mxGetPr(mxGetProperty(pShape,0,"len"))); } break; case DrakeCollision::MESH: { mxArray* pPoints; mexCallMATLAB(1,&pPoints,1,&pShape,"getPoints"); double* params = mxGetPr(pPoints); int n_params = (int) mxGetNumberOfElements(pPoints); for (int k=0; k<n_params; k++) params_vec.push_back(params[k]); mxDestroyArray(pPoints); // The element-to-link transform is applied in // RigidBodyMesh/getPoints - don't apply it again! T = Matrix4d::Identity(); } break; case DrakeCollision::CAPSULE: { params_vec.push_back(*mxGetPr(mxGetProperty(pShape,0,"radius"))); params_vec.push_back(*mxGetPr(mxGetProperty(pShape,0,"len"))); } break; default: // intentionally do nothing.. break; } model->addCollisionElement(i,T,shape,params_vec,group_name); if (model->bodies[i].parent<0) { model->updateCollisionElements(i); // update static objects only once - right here on load } } // Set collision filtering bitmasks pm = mxGetProperty(pBodies,i,"collision_filter"); const uint16_t* group = (uint16_t*)mxGetPr(mxGetField(pm,0,"belongs_to")); const uint16_t* mask = (uint16_t*)mxGetPr(mxGetField(pm,0,"collides_with")); //DEBUG //cout << "constructModelmex: Group: " << *group << endl; //cout << "constructModelmex: Mask " << *mask << endl; //END_DEBUG model->setCollisionFilter(i,*group,*mask); } } // THIS IS UGLY: I'm sending the terrain contact points into the // contact_pts field of the cpp RigidBody objects //DEBUG //cout << "constructModelmex: Parsing contact points " << endl; //cout << "constructModelmex: Get struct" << endl; //END_DEBUG mxArray* contact_pts_struct[1]; if (~mexCallMATLAB(1,contact_pts_struct,1,const_cast<mxArray**>(&pRBM),"getTerrainContactPoints")) { //DEBUG //cout << "constructModelmex: Got struct" << endl; //if (contact_pts_struct) { //cout << "constructModelmex: Struct pointer: " << contact_pts_struct << endl; //} else { //cout << "constructModelmex: Struct pointer NULL" << endl; //} //cout << "constructModelmex: Get numel of struct" << endl; //END_DEBUG const int n_bodies_w_contact_pts = mxGetNumberOfElements(contact_pts_struct[0]); //DEBUG //cout << "constructModelmex: Got numel of struct:" << n_bodies_w_contact_pts << endl; //END_DEBUG mxArray* pPts; int body_idx; int n_pts; for (int j=0; j < n_bodies_w_contact_pts; j++) { //DEBUG //cout << "constructModelmex: Loop: Iteration " << j << endl; //cout << "constructModelmex: Get body_idx" << endl; //END_DEBUG body_idx = (int) mxGetScalar(mxGetField(contact_pts_struct[0],j,"idx")) - 1; //DEBUG //cout << "constructModelmex: Got body_idx: " << body_idx << endl; //cout << "constructModelmex: Get points" << endl; //END_DEBUG pPts = mxGetField(contact_pts_struct[0],j,"pts"); //DEBUG //cout << "constructModelmex: Get points" << endl; //cout << "constructModelmex: Get number of points" << endl; //END_DEBUG n_pts = mxGetN(pPts); //DEBUG //cout << "constructModelmex: Got number of points: " << n_pts << endl; //cout << "constructModelmex: Set contact_pts of body" << endl; //END_DEBUG Map<MatrixXd> pts(mxGetPr(pPts),3,n_pts); model->bodies[body_idx].contact_pts.resize(4,n_pts); model->bodies[body_idx].contact_pts << pts, MatrixXd::Ones(1,n_pts); //DEBUG //cout << "constructModelmex: Contact_pts of body: " << endl; //cout << model->bodies[body_idx].contact_pts << endl; //END_DEBUG } } for (int i=0; i<model->num_frames; i++) { pm = mxGetProperty(pFrames,i,"name"); mxGetString(pm,buf,100); model->frames[i].name.assign(buf,strlen(buf)); pm = mxGetProperty(pFrames,i,"body_ind"); model->frames[i].body_ind = (int) mxGetScalar(pm)-1; pm = mxGetProperty(pFrames,i,"T"); memcpy(model->frames[i].T.data(),mxGetPr(pm),sizeof(double)*4*4); } memcpy(model->joint_limit_min.data(), mxGetPr(mxGetProperty(pRBM,0,"joint_limit_min")), sizeof(double)*model->num_dof); memcpy(model->joint_limit_max.data(), mxGetPr(mxGetProperty(pRBM,0,"joint_limit_max")), sizeof(double)*model->num_dof); const mxArray* a_grav_array = mxGetProperty(pRBM,0,"gravity"); if (a_grav_array && mxGetNumberOfElements(a_grav_array)==3) { double* p = mxGetPr(a_grav_array); model->a_grav[3] = p[0]; model->a_grav[4] = p[1]; model->a_grav[5] = p[2]; } else { mexErrMsgIdAndTxt("Drake:constructModelmex:BadGravity","Couldn't find a 3 element gravity vector in the object."); } model->compile(); plhs[0] = createDrakeMexPointer((void*)model,"RigidBodyManipulator"); }
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] ) { //DEBUG //mexPrintf("constructModelmex: START\n"); //END_DEBUG mxArray *pm; if (nrhs!=1) { mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","Usage model_ptr = constructModelmex(obj)"); } if (isa(prhs[0],"DrakeMexPointer")) { // then it's calling the destructor destroyDrakeMexPointer<RigidBodyManipulator*>(prhs[0]); return; } const mxArray* pRBM = prhs[0]; RigidBodyManipulator *model=new RigidBodyManipulator(); model->bodies.clear(); // a little gross: the default constructor makes a body "world". zap it because we will construct one again below // model->robot_name = get_strings(mxGetPropertySafe(pRBM,0,"name")); const mxArray* pBodies = mxGetPropertySafe(pRBM,0,"body"); if (!pBodies) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","the body array is invalid"); int num_bodies = static_cast<int>(mxGetNumberOfElements(pBodies)); const mxArray* pFrames = mxGetPropertySafe(pRBM,0,"frame"); if (!pFrames) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","the frame array is invalid"); int num_frames = static_cast<int>(mxGetNumberOfElements(pFrames)); for (int i = 0; i < num_bodies; i++) { //DEBUG //mexPrintf("constructModelmex: body %d\n",i); //END_DEBUG shared_ptr<RigidBody> b = make_shared<RigidBody>(); b->body_index = i; b->linkname = mxGetStdString(mxGetPropertySafe(pBodies, i, "linkname")); pm = mxGetPropertySafe(pBodies,i,"robotnum"); b->robotnum = (int) mxGetScalar(pm)-1; pm = mxGetPropertySafe(pBodies,i,"mass"); b->mass = mxGetScalar(pm); pm = mxGetPropertySafe(pBodies,i,"com"); if (!mxIsEmpty(pm)) memcpy(b->com.data(),mxGetPrSafe(pm),sizeof(double)*3); pm = mxGetPropertySafe(pBodies,i,"I"); if (!mxIsEmpty(pm)) memcpy(b->I.data(),mxGetPrSafe(pm),sizeof(double)*6*6); pm = mxGetPropertySafe(pBodies,i,"position_num"); b->position_num_start = (int) mxGetScalar(pm) - 1; //zero-indexed pm = mxGetPropertySafe(pBodies,i,"velocity_num"); b->velocity_num_start = (int) mxGetScalar(pm) - 1; //zero-indexed pm = mxGetPropertySafe(pBodies,i,"parent"); if (!pm || mxIsEmpty(pm)) b->parent = nullptr; else { int parent_ind = static_cast<int>(mxGetScalar(pm))-1; if (parent_ind >= static_cast<int>(model->bodies.size())) mexErrMsgIdAndTxt("Drake:constructModelmex:BadInputs","bad body.parent %d (only have %d bodies)",parent_ind,model->bodies.size()); if (parent_ind >= 0) b->parent = model->bodies[parent_ind]; } if (b->hasParent()) { string joint_name = mxGetStdString(mxGetPropertySafe(pBodies, i, "jointname")); //mexPrintf("adding joint %s\n", joint_name.c_str()); pm = mxGetPropertySafe(pBodies, i, "Ttree"); // todo: check that the size is 4x4 Isometry3d transform_to_parent_body; memcpy(transform_to_parent_body.data(), mxGetPrSafe(pm), sizeof(double) * 4 * 4); int floating = (int) mxGetScalar(mxGetPropertySafe(pBodies, i, "floating")); Eigen::Vector3d joint_axis; pm = mxGetPropertySafe(pBodies, i, "joint_axis"); memcpy(joint_axis.data(), mxGetPrSafe(pm), sizeof(double) * 3); double pitch = mxGetScalar(mxGetPropertySafe(pBodies, i, "pitch")); std::unique_ptr<DrakeJoint> joint; switch (floating) { case 0: { if (pitch == 0.0) { RevoluteJoint *revolute_joint = new RevoluteJoint(joint_name, transform_to_parent_body, joint_axis); joint = std::unique_ptr<RevoluteJoint>(revolute_joint); setLimits(pBodies, i, revolute_joint); setDynamics(pBodies, i, revolute_joint); } else if (std::isinf(static_cast<double>(pitch))) { PrismaticJoint *prismatic_joint = new PrismaticJoint(joint_name, transform_to_parent_body, joint_axis); joint = std::unique_ptr<PrismaticJoint>(prismatic_joint); setLimits(pBodies, i, prismatic_joint); setDynamics(pBodies, i, prismatic_joint); } else { joint = std::unique_ptr<HelicalJoint>(new HelicalJoint(joint_name, transform_to_parent_body, joint_axis, pitch)); } break; } case 1: { joint = std::unique_ptr<RollPitchYawFloatingJoint>(new RollPitchYawFloatingJoint(joint_name, transform_to_parent_body)); break; } case 2: { joint = std::unique_ptr<QuaternionFloatingJoint>(new QuaternionFloatingJoint(joint_name, transform_to_parent_body)); break; } default: { std::ostringstream stream; stream << "floating type " << floating << " not recognized."; throw std::runtime_error(stream.str()); } } b->setJoint(move(joint)); } //DEBUG //mexPrintf("constructModelmex: About to parse collision geometry\n"); //END_DEBUG pm = mxGetPropertySafe(pBodies,i,"collision_geometry"); Matrix4d T; if (!mxIsEmpty(pm)) { for (int j=0; j<mxGetNumberOfElements(pm); j++) { //DEBUG //cout << "constructModelmex: Body " << i << ", Element " << j << endl; //END_DEBUG mxArray* pShape = mxGetCell(pm,j); char* group_name_cstr = mxArrayToString(mxGetPropertySafe(pShape,0,"name")); string group_name; if (group_name_cstr) { group_name = group_name_cstr; } else { group_name = "default"; } // Get element-to-link transform from MATLAB object memcpy(T.data(), mxGetPrSafe(mxGetPropertySafe(pShape,0,"T")), sizeof(double)*4*4); auto shape = (DrakeShapes::Shape)static_cast<int>(mxGetScalar(mxGetPropertySafe(pShape,0,"drake_shape_id"))); vector<double> params_vec; RigidBody::CollisionElement element(T, b); switch (shape) { case DrakeShapes::BOX: { double* params = mxGetPrSafe(mxGetPropertySafe(pShape,0,"size")); element.setGeometry(DrakeShapes::Box(Vector3d(params[0],params[1],params[2]))); } break; case DrakeShapes::SPHERE: { double r(*mxGetPrSafe(mxGetPropertySafe(pShape,0,"radius"))); element.setGeometry(DrakeShapes::Sphere(r)); } break; case DrakeShapes::CYLINDER: { double r(*mxGetPrSafe(mxGetPropertySafe(pShape,0,"radius"))); double l(*mxGetPrSafe(mxGetPropertySafe(pShape,0,"len"))); element.setGeometry(DrakeShapes::Cylinder(r, l)); } break; case DrakeShapes::MESH: { string filename(mxArrayToString(mxGetPropertySafe(pShape,0,"filename"))); element.setGeometry(DrakeShapes::Mesh(filename, filename)); } break; case DrakeShapes::MESH_POINTS: { mxArray* pPoints; mexCallMATLAB(1,&pPoints,1,&pShape,"getPoints"); int n_pts = static_cast<int>(mxGetN(pPoints)); Map<Matrix3Xd> pts(mxGetPrSafe(pPoints),3,n_pts); element.setGeometry(DrakeShapes::MeshPoints(pts)); mxDestroyArray(pPoints); // The element-to-link transform is applied in // RigidBodyMesh/getPoints - don't apply it again! T = Matrix4d::Identity(); } break; case DrakeShapes::CAPSULE: { double r(*mxGetPrSafe(mxGetPropertySafe(pShape,0,"radius"))); double l(*mxGetPrSafe(mxGetPropertySafe(pShape,0,"len"))); element.setGeometry(DrakeShapes::Capsule(r, l)); } break; default: // intentionally do nothing.. //DEBUG //cout << "constructModelmex: SHOULD NOT GET HERE" << endl; //END_DEBUG break; } //DEBUG //cout << "constructModelmex: geometry = " << geometry.get() << endl; //END_DEBUG model->addCollisionElement(element, b, group_name); } // NOTE: the following should not be necessary since the same thing is being done in RigidBodyManipulator::compile, which is called below. // if (!model->bodies[i]->hasParent()) { // model->updateCollisionElements(model->bodies[i], cache); // update static objects only once - right here on load // } // Set collision filtering bitmasks pm = mxGetPropertySafe(pBodies,i,"collision_filter"); DrakeCollision::bitmask group, mask; mxArray* belongs_to = mxGetField(pm,0,"belongs_to"); mxArray* ignores = mxGetField(pm,0,"ignores"); if (!(mxIsLogical(belongs_to)) || !isMxArrayVector(belongs_to)) { cout << "is logical: " << mxIsLogical(belongs_to) << endl; cout << "number of dimensions: " << mxGetNumberOfDimensions(belongs_to) << endl; mexErrMsgIdAndTxt("Drake:constructModelmex:BadCollisionFilterStruct", "The 'belongs_to' field of the 'collision_filter' " "struct must be a logical vector."); } if (!(mxIsLogical(ignores)) || !isMxArrayVector(ignores)) { mexErrMsgIdAndTxt("Drake:constructModelmex:BadCollisionFilterStruct", "The 'ignores' field of the 'collision_filter' " "struct must be a logical vector."); } size_t numel_belongs_to(mxGetNumberOfElements(belongs_to)); size_t numel_ignores(mxGetNumberOfElements(ignores)); size_t num_collision_filter_groups = max(numel_belongs_to, numel_ignores); if (num_collision_filter_groups > MAX_NUM_COLLISION_FILTER_GROUPS) { mexErrMsgIdAndTxt("Drake:constructModelmex:TooManyCollisionFilterGroups", "The total number of collision filter groups (%d) " "exceeds the maximum allowed number (%d)", num_collision_filter_groups, MAX_NUM_COLLISION_FILTER_GROUPS); } mxLogical* logical_belongs_to = mxGetLogicals(belongs_to); for (int j = 0; j < numel_belongs_to; ++j) { if (static_cast<bool>(logical_belongs_to[j])) { group.set(j); } } mxLogical* logical_ignores = mxGetLogicals(ignores); for (int j = 0; j < numel_ignores; ++j) { if (static_cast<bool>(logical_ignores[j])) { mask.set(j); } } b->setCollisionFilter(group,mask); } model->bodies.push_back(b); } // THIS IS UGLY: I'm sending the terrain contact points into the // contact_pts field of the cpp RigidBody objects //DEBUG //mexPrintf("constructModelmex: Parsing contact points (calling getTerrainContactPoints)\n"); //cout << "constructModelmex: Get struct" << endl; //END_DEBUG mxArray* contact_pts_struct[1]; if (!mexCallMATLAB(1,contact_pts_struct,1,const_cast<mxArray**>(&pRBM),"getTerrainContactPoints")) { //DEBUG //mexPrintf("constructModelmex: Got terrain contact points struct\n"); //if (contact_pts_struct) { //cout << "constructModelmex: Struct pointer: " << contact_pts_struct << endl; //} else { //cout << "constructModelmex: Struct pointer NULL" << endl; //} //cout << "constructModelmex: Get numel of struct" << endl; //END_DEBUG const int n_bodies_w_contact_pts = static_cast<int>(mxGetNumberOfElements(contact_pts_struct[0])); //DEBUG //cout << "constructModelmex: Got numel of struct:" << n_bodies_w_contact_pts << endl; //END_DEBUG mxArray* pPts; int body_idx; int n_pts; for (int j=0; j < n_bodies_w_contact_pts; j++) { //DEBUG //cout << "constructModelmex: Loop: Iteration " << j << endl; //cout << "constructModelmex: Get body_idx" << endl; //END_DEBUG body_idx = (int) mxGetScalar(mxGetField(contact_pts_struct[0],j,"idx")) - 1; //DEBUG //cout << "constructModelmex: Got body_idx: " << body_idx << endl; //cout << "constructModelmex: Get points" << endl; //END_DEBUG pPts = mxGetField(contact_pts_struct[0],j,"pts"); //DEBUG //cout << "constructModelmex: Get points" << endl; //cout << "constructModelmex: Get number of points" << endl; //END_DEBUG n_pts = static_cast<int>(mxGetN(pPts)); //DEBUG //cout << "constructModelmex: Got number of points: " << n_pts << endl; //cout << "constructModelmex: Set contact_pts of body" << endl; //END_DEBUG Map<Matrix3Xd> pts(mxGetPrSafe(pPts),3,n_pts); model->bodies[body_idx]->contact_pts = pts; //DEBUG //mexPrintf("constructModelmex: created %d contact points for body %d\n", n_pts, body_idx); //cout << "constructModelmex: Contact_pts of body: " << endl; //cout << model->bodies[body_idx]->contact_pts << endl; //END_DEBUG } } // FRAMES //DEBUG //mexPrintf("constructModelmex: Parsing frames\n"); //END_DEBUG for (int i = 0; i < num_frames; i++) { shared_ptr<RigidBodyFrame> fr = make_shared<RigidBodyFrame>(); fr->name = mxGetStdString(mxGetPropertySafe(pFrames, i, "name")); pm = mxGetPropertySafe(pFrames,i,"body_ind"); fr->body = model->bodies[(int) mxGetScalar(pm)-1]; pm = mxGetPropertySafe(pFrames,i,"T"); memcpy(fr->transform_to_body.data(),mxGetPrSafe(pm),sizeof(double)*4*4); fr->frame_index = -i-2; model->frames.push_back(fr); } const mxArray* a_grav_array = mxGetPropertySafe(pRBM,0,"gravity"); if (a_grav_array && mxGetNumberOfElements(a_grav_array)==3) { double* p = mxGetPrSafe(a_grav_array); model->a_grav[3] = p[0]; model->a_grav[4] = p[1]; model->a_grav[5] = p[2]; } else { mexErrMsgIdAndTxt("Drake:constructModelmex:BadGravity","Couldn't find a 3 element gravity vector in the object."); } // LOOP CONSTRAINTS //DEBUG //mexPrintf("constructModelmex: Parsing loop constraints\n"); //END_DEBUG const mxArray* pLoops = mxGetPropertySafe(pRBM,0,"loop"); int num_loops = static_cast<int>(mxGetNumberOfElements(pLoops)); model->loops.clear(); for (int i=0; i<num_loops; i++) { pm = mxGetPropertySafe(pLoops,i,"frameA"); int frame_A_ind = static_cast<int>(-mxGetScalar(pm)-1); if (frame_A_ind<0 || frame_A_ind>=model->frames.size()) mexErrMsgIdAndTxt("Drake:constructModelmex:BadFrameNumber","Something is wrong, this doesn't point to a valid frame"); pm = mxGetPropertySafe(pLoops,i,"frameB"); int frame_B_ind = static_cast<int>(-mxGetScalar(pm)-1); if (frame_B_ind<0 || frame_B_ind>=model->frames.size()) mexErrMsgIdAndTxt("Drake:constructModelmex:BadFrameNumber","Something is wrong, this doesn't point to a valid frame"); pm = mxGetPropertySafe(pLoops,i,"axis"); Vector3d axis; memcpy(axis.data(), mxGetPrSafe(pm), 3*sizeof(double)); // cout << "loop " << i << ": frame_A = " << model->frames[frame_A_ind]->name << ", frame_B = " << model->frames[frame_B_ind]->name << endl; model->loops.push_back(RigidBodyLoop(model->frames[frame_A_ind],model->frames[frame_B_ind],axis)); } //ACTUATORS // LOOP CONSTRAINTS //DEBUG //mexPrintf("constructModelmex: Parsing actuators\n"); //END_DEBUG const mxArray* pActuators = mxGetPropertySafe(pRBM,0,"actuator"); int num_actuators = static_cast<int>(mxGetNumberOfElements(pActuators)); model->actuators.clear(); for (int i=0; i<num_actuators; i++) { string name = mxGetStdString(mxGetPropertySafe(pActuators,i,"name")); pm = mxGetPropertySafe(pActuators,i,"joint"); int joint = static_cast<int>(mxGetScalar(pm)-1); pm = mxGetPropertySafe(pActuators,i, "reduction"); model->actuators.push_back(RigidBodyActuator(name, model->bodies[joint], static_cast<double>(mxGetScalar(pm)))); } // LOOP CONSTRAINTS //DEBUG //mexPrintf("constructModelmex: Calling compile\n"); //END_DEBUG model->compile(); //mexPrintf("constructModelmex: Creating DrakeMexPointer\n"); plhs[0] = createDrakeMexPointer((void*)model,"RigidBodyManipulator"); //DEBUG //mexPrintf("constructModelmex: END\n"); //END_DEBUG }