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
}
