void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[])
{
if(nrhs != 5)
{
mexErrMsgIdAndTxt("Drake:testQuatmex:BadInputs","Usage [r,dr,e,ed,quat,dquat,q3,dq3,w,dw] = testQuatmex(q1,q2,axis,u,v)");
}
Vector4d q1;
Vector4d q2;
memcpy(q1.data(),mxGetPr(prhs[0]),sizeof(double)*4);
memcpy(q2.data(),mxGetPr(prhs[1]),sizeof(double)*4);
Vector4d r = quatDiff(q1,q2);
Matrix<double,4,8> dr = dquatDiff(q1,q2);
plhs[0] = mxCreateDoubleMatrix(4,1,mxREAL);
plhs[1] = mxCreateDoubleMatrix(4,8,mxREAL);
memcpy(mxGetPr(plhs[0]),r.data(),sizeof(double)*4);
memcpy(mxGetPr(plhs[1]),dr.data(),sizeof(double)*4*8);
Vector3d axis;
memcpy(axis.data(),mxGetPr(prhs[2]),sizeof(double)*3);
double e = quatDiffAxisInvar(q1,q2,axis);
Matrix<double,1,11> de = dquatDiffAxisInvar(q1,q2,axis);
plhs[2] = mxCreateDoubleScalar(e);
plhs[3] = mxCreateDoubleMatrix(1,11,mxREAL);
memcpy(mxGetPr(plhs[3]),de.data(),sizeof(double)*11);
Vector3d u,v;
Vector4d quat;
Matrix<double,4,6> dquat;
memcpy(u.data(),mxGetPr(prhs[3]),sizeof(double)*3);
memcpy(v.data(),mxGetPr(prhs[4]),sizeof(double)*3);
Vector4d q3 = quatProduct(q1,q2);
Matrix<double,4,8> dq3 = dquatProduct(q1,q2);
plhs[4] = mxCreateDoubleMatrix(4,1,mxREAL);
plhs[5] = mxCreateDoubleMatrix(4,8,mxREAL);
memcpy(mxGetPr(plhs[4]),q3.data(),sizeof(double)*4);
memcpy(mxGetPr(plhs[5]),dq3.data(),sizeof(double)*4*8);
Vector3d w = quatRotateVec(q1,u);
Matrix<double,3,7> dw = dquatRotateVec(q1,u);
plhs[6] = mxCreateDoubleMatrix(3,1,mxREAL);
plhs[7] = mxCreateDoubleMatrix(3,7,mxREAL);
memcpy(mxGetPr(plhs[6]),w.data(),sizeof(double)*3);
memcpy(mxGetPr(plhs[7]),dw.data(),sizeof(double)*3*7);
}
void drawCalibration()
{
double circleRadius=5.0;
glColor3fv(glRed);
glPointSize(5);
glBegin(GL_POINTS);
glVertex3d(0,0,focalDistance);
glVertex3dv(projPointEyeRight.data());
glEnd();
glPointSize(1);
// Draw the calibration circle
if ( pow(projPointEyeRight.x(),2)+pow(projPointEyeRight.y(),2) <= circleRadius*circleRadius )
{ timeInsideCircle++;
glColor3fv(glGreen50);
drawCircle(circleRadius,0,0,focalDistance);
if ( timeInsideCircle > 20 )
{
trialMode=FIXATIONMODE;
boost::thread okBeep( beepOk );
okBeep.detach();
sumOutside=0;
timeInsideCircle=0;
}
}
else
{ glColor3fv(glRed);
drawCircle(circleRadius,0,0,focalDistance);
}
}
/**
* @brief drawFixation
*/
void drawFixation()
{
double circleRadius = parameters.get("MaxCircleRadius"); // millimeters
double zBoundary = parameters.get("MaxZOscillation"); // millimeters
// Projection of view normal on the focal plane
Vector3d directionOfSight = (headEyeCoords.getRigidStart().getFullTransformation().linear()*Vector3d(0,0,-1)).normalized();
Eigen::ParametrizedLine<double,3> lineOfSightRight = Eigen::ParametrizedLine<double,3>::Through( headEyeCoords.getRightEye() , headEyeCoords.getRightEye()+directionOfSight );
Eigen::Hyperplane<double,3> focalPlane = Eigen::Hyperplane<double,3>::Through( Vector3d(1,0,focalDistance), Vector3d(0,1,focalDistance),Vector3d(0,0,focalDistance) );
double lineOfSightRightDistanceToFocalPlane = lineOfSightRight.intersection(focalPlane);
Vector3d opticalAxisToFocalPlaneIntersection = lineOfSightRightDistanceToFocalPlane *(directionOfSight)+ (headEyeCoords.getRightEye());
switch ( headCalibrationDone )
{
case 1:
{
// STIM_FIXED stimulus at (0,0,focalDistance)
glPushAttrib(GL_POINT_BIT);
glColor3fv(glRed);
glPointSize(5);
glBegin(GL_POINTS);
glVertex3d(0,0,focalDistance);
glVertex3d(headEyeCoords.getRightEye().x(),headEyeCoords.getRightEye().y(),focalDistance);
glEnd();
glPopAttrib();
break;
}
case 2:
{
// STIM_FIXED stimulus + projected points
glPushAttrib( GL_ALL_ATTRIB_BITS );
glPointSize(5);
glLineWidth(2);
glBegin(GL_POINTS);
glColor3fv(glRed);
glVertex3d(0,0,focalDistance);
glColor3fv(glBlue);
glVertex3dv(opticalAxisToFocalPlaneIntersection.data());
glColor3fv(glWhite);
glVertex3d(headEyeCoords.getRightEye().x(),headEyeCoords.getRightEye().y(),focalDistance);
glEnd();
double r2EyeRight = pow(headEyeCoords.getRightEye().x(),2)+pow(headEyeCoords.getRightEye().y(),2);
// Draw the calibration circle
if ( pow(opticalAxisToFocalPlaneIntersection.x(),2)+pow(opticalAxisToFocalPlaneIntersection.y(),2) <= circleRadius*circleRadius && abs(headEyeCoords.getRightEye().z()) < zBoundary && r2EyeRight<circleRadius*circleRadius )
{
readyToStart=true;
drawCircle(circleRadius,0,0,focalDistance,glGreen);
}
else
{
drawCircle(circleRadius,0,0,focalDistance,glRed);
}
glPopAttrib();
break;
}
}
}
void drawProbe()
{
glDisable(GL_COLOR_MATERIAL);
glDisable(GL_BLEND);
glDisable(GL_LIGHTING);
glColor3fv(glRed);
glPointSize(5);
glBegin(GL_POINTS); // the last position of projection point
glVertex3dv(projPointEyeRight.data());
glEnd();
glColor3fv(glRed);
glPointSize(1);
}
void drawTrial()
{
glPushAttrib(GL_COLOR_BUFFER_BIT | GL_POINT_BIT );
whichColor ? glColor3fv(glRed) : glColor3fv(glRed);
//glColor3fv(glRed);
glPointSize(15);
glBegin(GL_POINTS);
glVertex3dv(realIndex.data());
!whichColor ? glColor3fv(glRed) : glColor3fv(glRed);
glVertex3dv(reflectedIndex.data());
glEnd();
glPopAttrib();
/*
glPushMatrix();
glLoadIdentity();
glTranslated(0,0,focalDistance-100);
glRotated(mirrorAngle,0,1,0);
glScaled(-1,1,1);
stimDrawer.draw();
glPopMatrix();
*/
}
inline void vertex(const Vector3d& v)
{
#if USE_VERTEX_BUFFER
data[currentPosition++].segment<3>(0) = v.cast<float>();
++currentStripLength;
if (currentPosition == capacity)
{
flush();
data[0].segment<3>(0) = v.cast<float>();
currentPosition = 1;
currentStripLength = 1;
}
#else
glVertex3dv(v.data());
#endif
}
void drawFixation()
{
switch ( headCalibrationDone )
{
case 1:
// Fixed stimulus
glColor3fv(glWhite);
glDisable(GL_BLEND);
glPointSize(5);
glBegin(GL_POINTS);
glVertex3d(0,0,focalDistance);
glEnd();
glPointSize(1);
break;
case 2:
// Fixed stimulus + projected points
glColor3fv(glWhite);
glDisable(GL_BLEND);
glPointSize(5);
glBegin(GL_POINTS);
glVertex3d(0,0,focalDistance);
glColor3fv(glRed);
glVertex3dv(projPointEyeRight.data());
glColor3fv(glBlue);
glVertex3d(eyeRight.x(),eyeRight.y(),focalDistance);
glEnd();
glPointSize(1);
// Draw the calibration circle
glColor3fv(glWhite);
drawCircle(3,0,0,focalDistance);
break;
/*case 3:
// DRAW THE FIXATION POINT
glColor3fv(glRed);
glPushMatrix();
glTranslated(projPointEyeRight.x(),projPointEyeRight.y(),projPointEyeRight.z());
glutSolidSphere(1,10,10);
glPopMatrix();
break;
*/
}
}
void rigidBodyConstraintParse3dUnitVector(const mxArray* pm, Vector3d &unit_vec)
{
if(!mxIsNumeric(pm))
{
mexErrMsgIdAndTxt("Drake:rigidBodyConstraintParse3dUnitVector:BadInputs","vector should be a 3x1 double vector");
}
if(!(mxGetM(pm) == 3 && mxGetN(pm) == 1))
{
mexErrMsgIdAndTxt("Drake:rigidBodyConstraintParse3dUnitVector:BadInputs","vector should be of size 3x1");
}
memcpy(unit_vec.data(),mxGetPr(pm),sizeof(double)*3);
double vec_norm = unit_vec.norm();
if(vec_norm==0.0)
{
mexErrMsgIdAndTxt("Drake:rigidBodyConstraintParse3dUnitVector:BadInputs","The input cannot be a zero vector");
}
unit_vec = unit_vec/vec_norm;
};
void drawCalibration()
{
glPushAttrib(GL_ALL_ATTRIB_BITS);
double circleRadius=5.0;
glColor3fv(glRed);
glPointSize(5);
glBegin(GL_POINTS);
glVertex3d(0,0,focalDistance);
glVertex3dv(projPointEyeRight.data());
glEnd();
// Draw the calibration circle
if ( pow(projPointEyeRight.x(),2)+pow(projPointEyeRight.y(),2) <= circleRadius*circleRadius )
glColor3fv(glGreen50);
else
glColor3fv(glRed);
drawCircle(circleRadius,0,0,focalDistance);
glPopAttrib();
}
void CKinFuTracker::displayCameraPath() const{
vector<Eigen::Affine3d>::const_iterator cit = _v_T_cw_tracking.begin(); //from world to camera
glDisable(GL_LIGHTING);
glColor3f ( 0.f,0.f,1.f);
glLineWidth(2.f);
glBegin(GL_LINE_STRIP);
for (; cit != _v_T_cw_tracking.end(); cit++ )
{
SO3Group<double> mR = cit->linear();
Vector3d vT = cit->translation();
Vector3d vC = mR.inverse() *(-vT);
vC = mR.inverse()*(-vT);
glVertex3dv( vC.data() );
}
glEnd();
return;
}
void drawFixation()
{ switch ( headCalibrationDone )
{
case 1:
// Fixed stimulus at (0,0,focalDistance)
glPushAttrib(GL_POINT_BIT);
glColor3fv(glRed);
glPointSize(5);
glBegin(GL_POINTS);
glVertex3d(0,0,focalDistance);
glEnd();
glPopAttrib();
break;
case 2:
// Fixed stimulus + projected points
glPushAttrib(GL_POINT_BIT | GL_COLOR_BUFFER_BIT );
glColor3fv(glWhite);
glPointSize(5);
glBegin(GL_POINTS);
glVertex3d(0,0,focalDistance);
glColor3fv(glRed);
glVertex3dv(projPointEyeRight.data());
glColor3fv(glBlue);
glVertex3d(eyeRight.x(),eyeRight.y(),focalDistance);
glEnd();
glPopAttrib();
break;
case 3:
// Fixation point must be anchored to the orthogonal projection of eye right x coordinate
glPushAttrib(GL_COLOR_BUFFER_BIT); //Begin glPushAttrib colors
glColor3fv(glRed);
double circleRadius=str2num<double>(parameters.find("MaxCircleRadius")); // millimeters
glPushMatrix();
glLoadIdentity();
glTranslated(0,0,focalDistance);
glutSolidSphere(1,10,10);
glPopMatrix();
glPopAttrib(); // end glPopAttrib colors
break;
}
}
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
if (nrhs != 5) {
mexErrMsgIdAndTxt("Drake:testQuatmex:BadInputs", "Usage [r,dr,e,ed,quat,dquat,q3,dq3,w,dw] = testQuatmex(q1,q2,axis,u,v)");
}
Vector4d q1;
Vector4d q2;
memcpy(q1.data(), mxGetPr(prhs[0]), sizeof(double) * 4);
memcpy(q2.data(), mxGetPr(prhs[1]), sizeof(double) * 4);
Vector3d axis;
memcpy(axis.data(), mxGetPr(prhs[2]), sizeof(double) * 3);
Vector3d u,v;
memcpy(u.data(),mxGetPr(prhs[3]),sizeof(double)*3);
memcpy(v.data(),mxGetPr(prhs[4]),sizeof(double)*3);
{
auto autodiff_args = initializeAutoDiffTuple(q1, q2);
auto r_autodiff = quatDiff(get<0>(autodiff_args), get<1>(autodiff_args));
auto r = autoDiffToValueMatrix(r_autodiff);
auto dr = autoDiffToGradientMatrix(r_autodiff);
plhs[0] = mxCreateDoubleMatrix(4, 1, mxREAL);
plhs[1] = mxCreateDoubleMatrix(4, 8, mxREAL);
memcpy(mxGetPr(plhs[0]), r.data(), sizeof(double) * 4);
memcpy(mxGetPr(plhs[1]), dr.data(), sizeof(double) * 4 * 8);
}
{
auto autodiff_args = initializeAutoDiffTuple(q1, q2, axis);
auto e_autodiff = quatDiffAxisInvar(get<0>(autodiff_args), get<1>(autodiff_args), get<2>(autodiff_args));
auto e = e_autodiff.value();
auto de = e_autodiff.derivatives().transpose().eval();
plhs[2] = mxCreateDoubleScalar(e);
plhs[3] = mxCreateDoubleMatrix(1, 11, mxREAL);
memcpy(mxGetPr(plhs[3]), de.data(), sizeof(double) * 11);
}
{
auto autodiff_args = initializeAutoDiffTuple(q1, q2);
auto q3_autodiff = quatProduct(get<0>(autodiff_args), get<1>(autodiff_args));
auto q3 = autoDiffToValueMatrix(q3_autodiff);
auto dq3 = autoDiffToGradientMatrix(q3_autodiff);
plhs[4] = mxCreateDoubleMatrix(4, 1, mxREAL);
plhs[5] = mxCreateDoubleMatrix(4, 8, mxREAL);
memcpy(mxGetPr(plhs[4]), q3.data(), sizeof(double) * 4);
memcpy(mxGetPr(plhs[5]), dq3.data(), sizeof(double) * 4 * 8);
}
{
auto autodiff_args = initializeAutoDiffTuple(q1, u);
auto w_autodiff = quatRotateVec(get<0>(autodiff_args), get<1>(autodiff_args));
auto w = autoDiffToValueMatrix(w_autodiff);
auto dw = autoDiffToGradientMatrix(w_autodiff);
plhs[6] = mxCreateDoubleMatrix(3, 1, mxREAL);
plhs[7] = mxCreateDoubleMatrix(3, 7, mxREAL);
memcpy(mxGetPr(plhs[6]), w.data(), sizeof(double) * 3);
memcpy(mxGetPr(plhs[7]), dw.data(), sizeof(double) * 3 * 7);
}
}
void drawFixation()
{
glPushAttrib(GL_ALL_ATTRIB_BITS);
switch ( headCalibrationDone )
{
case 1:
// Fixed stimulus
glColor3fv(glWhite);
glDisable(GL_BLEND);
glPointSize(5);
glBegin(GL_POINTS);
glVertex3d(0,0,focalDistance);
glEnd();
glPointSize(1);
break;
case 2:
glColor3fv(glWhite);
glPointSize(5);
glBegin(GL_POINTS);
glVertex3d(0,0,focalDistance);
glColor3fv(glRed);
glVertex3dv(projPointEyeRight.data());
glColor3fv(glBlue);
glVertex3d(eyeRight.x(),eyeRight.y(),focalDistance);
glEnd();
// Draw the calibration circle
if ( (sqrt(pow(projPointEyeRight.x(),2)+pow(projPointEyeRight.y(),2)) < 3) && (sqrt( pow(eyeRight.x(),2) +pow(eyeRight.y(),2) ) < 3) )
{
canCalibrate=true;
glColor3fv(glGreen);
}
else
{
canCalibrate=false;
glColor3fv(glRed);
}
drawCircle(3,0,0,focalDistance);
break;
case 3:
{
/*
glPointSize(3);
glColor3fv(glRed);
glBegin(GL_POINTS);
glVertex3d(eyeRight.x(),0,focalDistance);
glVertex3d(-maxXOscillation,0,focalDistance);
glVertex3d(maxXOscillation,0,focalDistance);
glEnd();
*/
/*
glColor3fv(glWhite);
glPointSize(3);
glBegin(GL_POINTS);
glVertex3d(eyeRight.x(),eyeRight.y(),focalDistance);
glEnd();
if ( conditionInside )
glColor3fv(glGreen);
else
glColor3fv(glRed);
glBegin(GL_LINE_LOOP);
glVertex3d( -maxXOscillation,-maxXOscillation,focalDistance);
glVertex3d( maxXOscillation, -maxXOscillation, focalDistance);
glVertex3d( maxXOscillation, maxXOscillation, focalDistance);
glVertex3d( -maxXOscillation, maxXOscillation, focalDistance);
*/
glEnd();
}
break;
}
glPopAttrib();
}
double orient3d (Vector3d pa, Vector3d pb, Vector3d pc, Vector3d pd) {
initExact();
return (double) orient3d(pa.data(), pb.data(), pc.data(), pd.data());
}
void drawFixation()
{
switch ( headCalibrationDone )
{
case 1:
// Fixed stimulus
glColor3fv(glWhite);
glDisable(GL_BLEND);
glPointSize(5);
glBegin(GL_POINTS);
glVertex3d(0,0,focalDistance);
glEnd();
glPointSize(1);
break;
case 2:
// Fixed stimulus + projected points
glColor3fv(glWhite);
glDisable(GL_BLEND);
glPointSize(5);
glBegin(GL_POINTS);
glVertex3d(0,0,focalDistance);
glColor3fv(glRed);
glVertex3dv(projPointEyeRight.data());
glColor3fv(glBlue);
glVertex3d(eyeRight.x(),eyeRight.y(),focalDistance);
glEnd();
glPointSize(1);
// Draw the calibration circle
glColor3fv(glWhite);
break;
case 3:
{
// DRAW THE FIXATION POINT
double eyeToCenterAngleX= toDegrees(atan(eyeRight.x()/(-focalDistance-eyeRight.z()) ));
double eyeToCenterAngleY= toDegrees(atan(eyeRight.y()/(-focalDistance-eyeRight.z()) ));
double projPointAngleX = toDegrees( atan( (projPointEyeRight.x()-eyeRight.x())/abs(projPointEyeRight.z())));
double maxAllowedTranslationYaw = str2num<double>(parameters.find("MaxAllowedTranslationYaw"));
Vector3d stimulusCenter(0,0,0);
Matrix3d objrotation = Matrix3d::Identity();
// IMPORTANT Reset the previous status of transformation
objectActiveTransformation.setIdentity();
switch ( (int) factors["Rotation"] )
{
case 2:
{
objrotation = (AngleAxis<double>(eulerAngles.getYaw()*factors["FollowingSpeed"], Vector3d::UnitY())
*AngleAxis<double>(eulerAngles.getPitch(), Vector3d::UnitX())).toRotationMatrix();
instantPlaneSlant = toDegrees(eulerAngles.getYaw())*factors["RotationSpeed"]+factors["Slant"];
stimulusCenter = objrotation*Vector3d(0,0,focalDistance)+headEyeCoords.getRigidStart().getFullTransformation().translation();
objectActiveTransformation.linear()=objrotation;
}
break;
case 1:
{
objrotation = (AngleAxis<double>(eulerAngles.getYaw(), Vector3d::UnitY())
*AngleAxis<double>(eulerAngles.getPitch()*factors["FollowingSpeed"], Vector3d::UnitX())).toRotationMatrix();
instantPlaneSlant = toDegrees(eulerAngles.getPitch())*factors["RotationSpeed"]+factors["Slant"];
stimulusCenter = objrotation*Vector3d(0,0,focalDistance)+headEyeCoords.getRigidStart().getFullTransformation().translation();
objectActiveTransformation.linear()=objrotation;
}
break;
case 0:
{
objrotation = (AngleAxis<double>(eulerAngles.getYaw(), Vector3d::UnitY())
*AngleAxis<double>(eulerAngles.getPitch(), Vector3d::UnitX())).toRotationMatrix();
//instantPlaneSlant = eyeRight.x()*factors["RotationSpeed"]/10+factors["Slant"];
instantPlaneSlant = toDegrees( atan(eyeRight.x()/abs(focalDistance+eyeRight.z()) ) )*factors["RotationSpeed"]+factors["Slant"];
stimulusCenter = headEyeCoords.getRigidStart().getFullTransformation().linear()*Vector3d(eyeRight.x()*factors["FollowingSpeed"],eyeRight.y(),eyeRight.z()+focalDistance);
objectActiveTransformation.linear() = objrotation;
}
break;
}
objectActiveTransformation.translation() = stimulusCenter;
Vector3d fixationPointTmp = objectActiveTransformation.translation();
glPushMatrix();
glTranslated(fixationPointTmp.x(),fixationPointTmp.y(),fixationPointTmp.z());
glutSolidSphere(1,10,10);
glPopMatrix();
break;
}
}
}
bool Localization::Calculate(vector<LineSegment> &clusteredLines,
bool circleDetected, const Point2f &FieldHullRealCenter,
const vector<cv::Point2f> &FieldHullReal,
const Point2d &resultCircleRotated, const vector<Point2f> &goalPosition,
const bool &confiused, vector<LineContainer> &AllLines,
vector<FeatureContainer> &AllFeatures)
{
if (_cameraProjections == NULL)
{
ROS_ERROR("Error in programming");
return false;
}
AllLines.reserve(clusteredLines.size());
AllFeatures.reserve(5);
const double MAX_FASHER = 200.;
atLeastOneObservation = false;
double UPDATENORMAL = params.loc->UPDATENORMAL()
* params.loc->TOTALGAIN();
double UPDATESTRONG = params.loc->UPDATESTRONG()
* params.loc->TOTALGAIN();
double UPDATEWEAK = params.loc->UPDATEWEAK() * params.loc->TOTALGAIN();
LineSegment HorLine(cv::Point(0, -10), cv::Point(0, 10));
LineSegment VerLine(cv::Point(10, 0), cv::Point(-10, 0));
for (size_t i = 0; i < clusteredLines.size(); i++)
{
LineSegment lineSeg = clusteredLines[i];
if (lineSeg.GetLength() > params.loc->minLineLen())
{
cv::Point2d mid = lineSeg.GetMiddle();
if (lineSeg.GetAbsMinAngleDegree(VerLine) < 45)
{
LineType thisType = VerUndef;
double angleDiffVer = lineSeg.GetExteriorAngleDegree(VerLine);
if (angleDiffVer < -90)
angleDiffVer += 180;
if (angleDiffVer > 90)
angleDiffVer += -180;
if (lineSeg.DistanceFromLine(cv::Point(0, 0))
> params.loc->VerLineMinDistanceToUpdate())
{
LandmarkType ltype = CenterL;
double estimatedY = 0;
if (mid.y > 0)
{
thisType = VerLeft;
estimatedY = B2 - mid.y;
ltype = LeftL;
}
else
{
thisType = VerRight;
estimatedY = -B2 + abs(mid.y);
ltype = RightL;
}
addObservation(Point2d(0, estimatedY), 0,
MAX_FASHER * getUpdateCoef(UPDATENORMAL, lineSeg),
ltype);
}
else if (lineSeg.DistanceFromLine(FieldHullRealCenter)
> (params.loc->VerLineMinDistanceToUpdate() / 2.)
&& cv::contourArea(FieldHullReal) > 4)
{
LandmarkType ltype = CenterL;
LineSegment l2Test = lineSeg;
if (lineSeg.P1.x > lineSeg.P2.x)
{
l2Test.P1 = lineSeg.P2;
l2Test.P2 = lineSeg.P1;
}
double estimatedY = 0;
if (l2Test.GetSide(FieldHullRealCenter) < 0)
{
thisType = VerLeftNear;
estimatedY = B2 - mid.y;
ltype = LeftL;
}
else
{
thisType = VerRightNear;
estimatedY = -B2 + abs(mid.y);
ltype = RightL;
}
addObservation(Point2d(0, estimatedY), 0,
MAX_FASHER * getUpdateCoef(UPDATENORMAL, lineSeg),
ltype);
}
AllLines.push_back(LineContainer(lineSeg, thisType));
}
else
{
LineType thisType = HorUndef;
double angleDiffHor = lineSeg.GetExteriorAngleDegree(HorLine);
if (angleDiffHor < -90)
angleDiffHor += 180;
if (angleDiffHor > 90)
angleDiffHor += -180;
if (circleDetected
&& DistanceFromLineSegment(lineSeg, resultCircleRotated)
< 1)
{
thisType = HorCenter;
double estimatedX = -mid.x;
addObservation(Point2d(estimatedX, 0),
MAX_FASHER * UPDATENORMAL, 0, CenterL);
}
if (goalPosition.size() >= 2
&& lineSeg.DistanceFromLine(goalPosition[0]) < 0.5
&& lineSeg.DistanceFromLine(goalPosition[1]) < 0.5)
{
LandmarkType typel = CenterL;
double estimatedX = 0;
if (mid.x > 0)
{
typel = FrontL;
estimatedX = A2 - mid.x;
}
else
{
typel = BackL;
estimatedX = -A2 + abs(mid.x);
}
double lowPC = getUpdateCoef(
(goalPosition.size() == 2 ?
UPDATESTRONG : UPDATENORMAL), lineSeg);
addObservation(Point2d(estimatedX, 0), MAX_FASHER * lowPC,
0, typel);
thisType = HorGoal;
}
AllLines.push_back(LineContainer(lineSeg, thisType));
}
}
}
for (size_t i = 0; i < AllLines.size(); i++)
{
LineContainer hI = AllLines[i];
if (hI.type != HorUndef)
continue;
for (size_t j = i; j < AllLines.size(); j++)
{
LineContainer hJ = AllLines[j];
if (hJ.type != HorUndef)
continue;
cv::Point2d midI = hI.lineTransformed.GetMiddle();
cv::Point2d midJ = hJ.lineTransformed.GetMiddle();
double distanceToEachOther = dist3D_Segment_to_Segment(
hI.lineTransformed, hJ.lineTransformed);
double midPointsLSAngleToOne =
hI.lineTransformed.GetAbsMinAngleDegree(
LineSegment(midI, midJ));
if (distanceToEachOther < E * 1.5 && distanceToEachOther > E * 0.5
&& hI.lineTransformed.GetAbsMinAngleDegree(
hJ.lineTransformed) < 30
&& midPointsLSAngleToOne > 25)
{
bool hJ_Is_Goal_Line = hJ.lineTransformed.DistanceFromLine(
cv::Point(0, 0))
> hI.lineTransformed.DistanceFromLine(cv::Point(0, 0));
cv::Point2d mid = hJ_Is_Goal_Line ? midJ : midI;
double estimatedX = 0;
if ((hJ_Is_Goal_Line ? hJ.lineTransformed : hI.lineTransformed).DistanceFromLine(
cv::Point(0, 0)) > 1.2)
{
LandmarkType typel = CenterL;
if (mid.x > 0)
{
estimatedX = A2 - mid.x;
typel = FrontL;
}
else
{
estimatedX = -A2 + abs(mid.x);
typel = BackL;
}
double lowPC = getUpdateCoef(UPDATESTRONG,
hJ_Is_Goal_Line ?
hJ.lineTransformed : hI.lineTransformed);
addObservation(Point2d(estimatedX, 0), MAX_FASHER * lowPC,
0, typel);
}
}
}
}
if (circleDetected)
{
double estimatedX = -resultCircleRotated.x;
double estimatedY = -resultCircleRotated.y;
addObservation(Point2d(estimatedX, estimatedY),
MAX_FASHER * UPDATEWEAK, MAX_FASHER * UPDATEWEAK, CenterL);
}
if (atLeastOneObservation)
{
updateVertexIdx();
if ((nodeCounter % 30 == 0) && PreviousVertexId > 0)
{
optimizer.initializeOptimization();
optimizer.optimize(1);
Vector3d tmpV;
optimizer.vertex(PreviousVertexId)->getEstimateData(tmpV.data());
location.x = tmpV(0);
location.y = tmpV(1);
}
}
return true;
}
void drawGeneratedPlanes()
{
Hyperplane<double,3> screenPlane,mirrorPlane;
//mirror[3] = Vector3d(mirror[1].x(),mirror[2].y(),mirror[1].z());
screen[3] = Vector3d(screen[1].x(), screen[2].y(),screen[1].z());
screenPlane = Hyperplane<double,3>::Through( screen[0], screen[1],screen[2] );
mirrorPlane = Hyperplane<double,3>::Through(mirror[1], mirror[0], mirror[2] );
screenNormal = screenPlane.normal();
mirrorNormal = mirrorPlane.normal();
Eigen::ParametrizedLine<double,3> line = Eigen::ParametrizedLine<double,3>::Through( screenCenter , screenCenter + screenNormal*10);
// Calcola il punto proiezione della normale uscente dallo schermo sullo specchio
double p = line.intersection(mirrorPlane);
screenProjectionOnMirror = p*((screenNormal).normalized()) + screenCenter;
planesIntersectionBase = -getPlaneIntersection(mirrorPlane,screenPlane,0);
planesIntersectionDirection = mirrorPlane.normal().cross(screenPlane.normal());
planesIntersecEnd = planesIntersectionBase - 200*planesIntersectionDirection;
// Draw the planes intersection
glPushMatrix();
glColor3fv(glBlue);
glBegin(GL_LINES);
glVertex3dv(planesIntersectionBase.data());
glVertex3dv(planesIntersecEnd.data());
glEnd();
glPopMatrix();
// Draw the mirror plane (only if calibrationPhase==0)
if ( calibrationPhase==0 )
{
glPushMatrix();
Grid gridMirror;
gridMirror.setRowsAndCols(20,20);
gridMirror.init(mirror[1],mirror[0],mirror[2],mirror[3]); // invert the order to have 45 degrees yaw and 90 degree pitch when correct
glColor3fv(glRed);
gridMirror.draw();
glPopMatrix();
// Draw the mirror normal
mirrorCenter = (mirror[0]+mirror[1]+mirror[2]+mirror[3])/4.0;
Vector3d mirrorCenterDirection = mirrorCenter + 20*mirrorNormal;
glPushMatrix();
glColor3fv(glRed);
glBegin(GL_LINES);
glVertex3dv(mirrorCenter.data());
glVertex3dv(mirrorCenterDirection.data());
glEnd();
glPopMatrix();
}
// Draw the screen plane
if ( calibrationPhase!=0)
{
glPushMatrix();
Grid gridScreen;
gridScreen.setRowsAndCols(20,20);
gridScreen.init(screen[0],screen[1],screen[2],screen[3]);
glColor3fv(glGreen);
gridScreen.draw();
glPopMatrix();
// Draw the screen normal
screenCenter = (screen[0]+screen[1]+screen[2]+screen[3])/4.0;
Vector3d screenCenterDirection = screenCenter + 20*screenNormal;
glPushMatrix();
glBegin(GL_LINES);
glVertex3dv(screenCenter.data());
glVertex3dv(screenCenterDirection.data());
glEnd();
glPopMatrix();
}
// Draw the vector from screen center to mirror center
glPushMatrix();
glColor3fv(glRed);
glBegin(GL_LINES);
glVertex3dv(screenCenter.data());
glVertex3dv(mirrorCenter.data());
glEnd();
glPopMatrix();
// Draw the vector from mirror center to X axis
Vector3d orthoProjMirrorCenter(mirrorCenter);
glPushMatrix();
glColor3fv(glRed);
glBegin(GL_LINES);
glVertex3dv(mirrorCenter.data());
glVertex3d(mirrorCenter.x(),mirrorCenter.y(),0);
glEnd();
glPopMatrix();
// draw mirror saved center
if ( calibrationPhase == 1)
{
glPushMatrix();
glTranslatef(mirrorSavedCenter.x(),mirrorSavedCenter.y(),mirrorSavedCenter.z());
glutSolidSphere(1,10,20);
glPopMatrix();
}
mirrorYaw = mathcommon::toDegrees(acos(mirrorNormal.z()));
mirrorPitch = mathcommon::toDegrees(acos(mirrorNormal.y()));
screenYaw = mathcommon::toDegrees(acos(screenNormal.z()));
screenPitch = mathcommon::toDegrees(acos(screenNormal.y()));
glColor3fv(glWhite);
double tolerance = 0.5; //degrees
bool mirrorYawOK = abs(mirrorYaw - 45) <= tolerance;
bool screenYawOK = abs(screenYaw - 90) <= tolerance;
bool mirrorPitchOK = abs(mirrorPitch-90) <= tolerance;
bool screenPitchOK = abs(screenPitch-90) <= tolerance;
GLText text;
text.init(SCREEN_WIDTH,SCREEN_HEIGHT,glWhite,GLUT_BITMAP_HELVETICA_18);
text.enterTextInputMode();
switch( calibrationPhase )
{
case 0:
text.draw("==== INSTRUCTIONS ====");
text.draw("Put the markers on the mirror vertices, try to keep them aligned. Press SPACEBAR to save their coordinates");
text.draw("Press 2/8 to zoom out/in. Press '+/-' to go forward or backward");
break;
case 1:
{
text.draw("Now remove the mirror, mirror coordinates are saved to calibrationCoordinates.txt");
text.draw("Now you must put the markers on the screen");
text.draw("");
text.draw("");
text.draw("Mirror current center= " + util::stringify< Eigen::Matrix<double,1,3> >(mirrorSavedCenter.transpose() ) );
text.draw("Distance from mirror center to screen center " + util::stringify<double>( (mirrorSavedCenter - screenCenter).norm() ));
text.draw("Projection error of screen normal on mirror center " + util::stringify<Eigen::Matrix<double,1,3> >( screenProjectionOnMirror.transpose() ) );
}
break;
case 2:
break;
}
text.draw("==== MIRROR INFO ====");
mirrorPitchOK ? glColor3fv(glGreen) : glColor3fv(glRed);
text.draw("Mirror Pitch= " + util::stringify<double>(mirrorPitch) );
mirrorYawOK ? glColor3fv(glGreen) : glColor3fv(glRed);
text.draw("Mirror Yaw=" + util::stringify<double>(mirrorYaw));
glColor3fv(glWhite);
text.draw("==== SCREEN INFO ====");
screenPitchOK ? glColor3fv(glGreen) : glColor3fv(glRed);
text.draw("Screen Pitch=" + util::stringify<double>(screenPitch));
screenYawOK ? glColor3fv(glGreen) : glColor3fv(glRed);
text.draw("Screen Yaw=" + util::stringify<double>(screenYaw));
// Here print the relative orientation of screen and mirror
if ( calibrationPhase==0)
{ text.draw("Total distance (norm)= " + util::stringify<double>(abs(mirrorCenter.z())+(mirrorCenter - screenCenter).norm()) );
text.draw("Total distance (only x+z)= " + util::stringify<double>( abs(mirrorCenter.x()-screenCenter.x()) + abs(mirrorCenter.z()) ));
}
else
{
text.draw("Total distance (norm)= " + util::stringify<double>(abs(mirrorSavedCenter.z())+(mirrorSavedCenter - screenCenter).norm()) );
text.draw("Total distance (ignore y offset)= " + util::stringify<double>( abs(mirrorSavedCenter.x()-screenCenter.x()) + abs(mirrorSavedCenter.z()) ));
}
text.draw("Intersection of planes" + util::stringify<Eigen::Matrix<int,1,3> >(planesIntersectionBase.cast<int>()));
text.leaveTextInputMode();
}
void symmetrize()
{
using namespace std;
using namespace igl;
using namespace Eigen;
if(s.sel.size() == 0)
{
cout<<YELLOWGIN("Make a selection first.")<<endl;
return;
}
push_undo();
push_scene();
push_object();
Vector3d right;
right_axis(right.data(),right.data()+1,right.data()+2);
right.normalize();
MatrixXd RC(s.C.rows(),s.C.cols());
MatrixXd old_C = s.C;
for(int c = 0;c<s.C.rows();c++)
{
const Vector3d Cc = s.C.row(c);
const auto A = Cc-Vcen;
const auto A1 = A.dot(right) * right;
const auto A2 = A-A1;
RC.row(c) = Vcen + A2 - A1;
}
vector<bool> mask = selection_mask(s.sel,s.C.rows());
// stupid O(n²) matching
for(int c = 0;c<s.C.rows();c++)
{
// not selected
if(!mask[c])
{
continue;
}
const Vector3d Cc = s.C.row(c);
int min_r = -1;
double min_dist = 1e25;
double max_dist = 0.1*bbd;
for(int r= 0;r<RC.rows();r++)
{
const Vector3d RCr = RC.row(r);
const double dist = (Cc-RCr).norm();
if(
dist<min_dist && // closest
dist<max_dist && // not too far away
(c==r || (Cc-Vcen).dot(right)*(RCr-Vcen).dot(right) > 0) // on same side
)
{
min_dist = dist;
min_r = r;
}
}
if(min_r>=0)
{
if(mask[min_r])
{
s.C.row(c) = 0.5*(Cc.transpose()+RC.row(min_r));
}else
{
s.C.row(c) = RC.row(min_r);
}
}
}
pop_object();
pop_scene();
}
void MapInitializer::addObservation(Vector3d & X, Vector2d pt, Transformation<double> & pose,
const ICamera * cam, const Transformation<double> & TbaseCam)
{
CostFunction * costFunc = new ReprojectionErrorStereo(pt, TbaseCam, cam);
problem.AddResidualBlock(costFunc, NULL, X.data(), pose.transData(), pose.rotData());
}
double index2k(const indices3d& ilist) {
Vector3d tmp;
transform(ilist.begin(), ilist.end(), tmp.data(), ki);
return tmp.norm();
}
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 drawStimulus()
{
double stimRadius = factors.getCurrent().at("StimulusRadius");
glPushMatrix();
glPointSize(7);
glColor3fv(glRed);
/*
glPushMatrix();
glLoadMatrixd(rigidStartThumb.getFullTransformation().data());
glScaled(1,1,0.001);
glutSolidSphere(thumbSphereRadius,15,15);
glPopMatrix();
glPushMatrix();
glLoadMatrixd(rigidStartIndex.getFullTransformation().data());
glScaled(0.001,1,1);
glutSolidSphere(indexSphereRadius,15,15);
glPopMatrix();
*/
glPushMatrix();
glTranslated(-10,0,0);
glMultMatrixd(rigidStartThumb.getFullTransformation().data());
glRotated(90,0,1,0);
ddcylinder();
glPopMatrix();
glPushMatrix();
glTranslated(5,0,-10);
glMultMatrixd(rigidStartIndex.getFullTransformation().data());
ddcylinder();
glPopMatrix();
////////// OLD ////////
/*
glPointSize(7);
glBegin(GL_POINTS);
glVertex3dv(visualThumb.data());
glVertex3dv(visualIndex.data());
glEnd();
*/
double insideRadius = factors.getCurrent().at("DisappearRadius");
if ((thumb - visualStimCenter).norm() > insideRadius ) // disegna lo stimolo
{
glColor3fv(glRed);
glPushMatrix();
glLoadIdentity();
glTranslated(visualStimCenter.x(),visualStimCenter.y(),visualStimCenter.z());
stimDrawer.draw();
glScaled(0.99,0.99,0.99);
glColor3fv(glBlack);
glutSolidSphere(stimRadius,20,20);
glPopMatrix();
glPushAttrib(GL_ALL_ATTRIB_BITS);
glLineWidth(5);
glColor3fv(glRed);
glBegin(GL_LINES);
glVertex3dv(rodStart.data());
glVertex3dv(rodEnd.data());
glEnd();
glPopAttrib();
}
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
if(!mxIsNumeric(prhs[0]))
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","prhs[0] should be the constraint type");
}
int constraint_type = (int) mxGetScalar(prhs[0]);
switch(constraint_type)
{
// QuasiStaticConstraint
case RigidBodyConstraint::QuasiStaticConstraintType:
{
if(nrhs != 2 && nrhs!= 3 && nrhs != 4)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Usage ptr = constructPtrPtrRigidBodyConstraintmex(RigidBodyConstraint::QuasiStaticConstraintType,robot_ptr,tspan,robotnum)");
}
RigidBodyManipulator* model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
Vector2d tspan;
int* robotnum;
int num_robot;
if(nrhs <= 3)
{
num_robot = 1;
robotnum = new int[num_robot];
robotnum[0] = 0;
}
else
{
num_robot = mxGetNumberOfElements(prhs[3]);
double* robotnum_tmp = new double[num_robot];
robotnum = new int[num_robot];
memcpy(robotnum_tmp,mxGetPr(prhs[3]),sizeof(double)*num_robot);
for(int i = 0;i<num_robot;i++)
{
robotnum[i] = (int) robotnum_tmp[i]-1;
}
delete[] robotnum_tmp;
}
if(nrhs<=2)
{
tspan<<-mxGetInf(),mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[2],tspan);
}
set<int> robotnumset(robotnum,robotnum+num_robot);
QuasiStaticConstraint* cnst = new QuasiStaticConstraint(model,tspan,robotnumset);
plhs[0] = createDrakeConstraintMexPointer((void*) cnst,"deleteRigidBodyConstraintmex","QuasiStaticConstraint");
delete[] robotnum;
}
break;
// PostureConstraint
case RigidBodyConstraint::PostureConstraintType:
{
if(nrhs != 2 && nrhs != 3)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Usage ptr = constructPtrRigidBodyConstraintmex(RigidBodyConstraint::PostureConstraintType,robot.mex_model_ptr,tspan)");
}
Vector2d tspan;
if(nrhs == 2)
{
tspan<<-mxGetInf(),mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[2],tspan);
}
RigidBodyManipulator* robot = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
PostureConstraint* cnst = new PostureConstraint(robot,tspan);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst,"deleteRigidBodyConstraintmex","PostureConstraint");
}
break;
// SingleTimeLinearPostureConstraint
case RigidBodyConstraint::SingleTimeLinearPostureConstraintType:
{
if(nrhs != 7 && nrhs != 8)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Usage ptr = constructPtrRigidBodyConstraintmex(RigidBodyConstraint::SingleTimeLinearPostureConstraintType,robot.mex_model_ptr,iAfun,jAvar,A,lb,ub,tspan");
}
RigidBodyManipulator* model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
Vector2d tspan;
if(nrhs<=7)
{
tspan<<-mxGetInf(),mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[7],tspan);
}
if(!mxIsNumeric(prhs[2]) || !mxIsNumeric(prhs[3]) || !mxIsNumeric(prhs[4]))
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","iAfun, jAvar and A must be numeric");
}
int lenA = mxGetM(prhs[2]);
if(mxGetM(prhs[3]) != lenA || mxGetM(prhs[4]) != lenA || mxGetN(prhs[2]) != 1 || mxGetN(prhs[3]) != 1 || mxGetN(prhs[4]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","iAfun,jAvar,A must be column vectors of the same size");
}
VectorXi iAfun(lenA);
VectorXi jAvar(lenA);
VectorXd A(lenA);
for(int i = 0;i<lenA;i++)
{
iAfun(i) = (int) *(mxGetPr(prhs[2])+i)-1;
jAvar(i) = (int) *(mxGetPr(prhs[3])+i)-1;
A(i) = *(mxGetPr(prhs[4])+i);
}
if(!mxIsNumeric(prhs[5]) || !mxIsNumeric(prhs[6]) || mxGetM(prhs[5]) != mxGetM(prhs[6]) || mxGetN(prhs[5]) != 1 || mxGetN(prhs[6]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","lb and ub must be numeric column vectors of the same size");
}
int num_constraint = mxGetM(prhs[5]);
VectorXd lb(num_constraint);
VectorXd ub(num_constraint);
memcpy(lb.data(),mxGetPr(prhs[5]),sizeof(double)*num_constraint);
memcpy(ub.data(),mxGetPr(prhs[6]),sizeof(double)*num_constraint);
SingleTimeLinearPostureConstraint* cnst = new SingleTimeLinearPostureConstraint(model,iAfun,jAvar,A,lb,ub,tspan);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst,"deleteRigidBodyConstraintmex","SingleTimeLinearPostureConstraint");
}
break;
// AllBodiesClosestDistanceConstraint
case RigidBodyConstraint::AllBodiesClosestDistanceConstraintType:
{
//DEBUG
//cout << "nrhs = " << nrhs << endl;
//END_DEBUG
if(nrhs != 5 && nrhs != 6)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs",
"Usage ptr = constructPtrRigidBodyConstraintmex(RigidBodyConstraint::AllBodiesClosestDistanceConstraintType, robot.mex_model_ptr,lb,ub,active_collision_options,tspan)");
}
RigidBodyManipulator* model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
Vector2d tspan;
if(nrhs == 5)
{
tspan<< -mxGetInf(), mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[5],tspan);
}
double lb = (double) mxGetScalar(prhs[2]);
double ub = (double) mxGetScalar(prhs[3]);
// Parse `active_collision_options`
vector<int> active_bodies_idx;
set<string> active_group_names;
// First get the list of body indices for which to compute distances
const mxArray* active_collision_options = prhs[4];
const mxArray* body_idx = mxGetField(active_collision_options,0,"body_idx");
if (body_idx != NULL) {
//DEBUG
//cout << "collisionDetectmex: Received body_idx" << endl;
//END_DEBUG
int n_active_bodies = mxGetNumberOfElements(body_idx);
//DEBUG
//cout << "collisionDetectmex: n_active_bodies = " << n_active_bodies << endl;
//END_DEBUG
active_bodies_idx.resize(n_active_bodies);
memcpy(active_bodies_idx.data(),(int*) mxGetData(body_idx),
sizeof(int)*n_active_bodies);
transform(active_bodies_idx.begin(),active_bodies_idx.end(),
active_bodies_idx.begin(),
[](int i){return --i;});
}
// Then get the group names for which to compute distances
const mxArray* collision_groups = mxGetField(active_collision_options,0,
"collision_groups");
if (collision_groups != NULL) {
for (const string& str : get_strings(collision_groups)) {
active_group_names.insert(str);
}
}
auto cnst = new AllBodiesClosestDistanceConstraint(model,lb,ub,
active_bodies_idx,active_group_names,tspan);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst,"deleteRigidBodyConstraintmex",
"AllBodiesClosestDistanceConstraint");
}
break;
// WorldEulerConstraint
case RigidBodyConstraint::WorldEulerConstraintType:
{
if(nrhs != 6 && nrhs != 5)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Usage ptr = constructPtrRigidBodyConstraintmex(RigidBodyConstraint::WorldEulerConstraintType,robot.mex_model_ptr,body,lb,ub,tspan)");
}
RigidBodyManipulator* model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
WorldEulerConstraint* cnst = nullptr;
Vector2d tspan;
if(nrhs == 5)
{
tspan<< -mxGetInf(), mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[5],tspan);
}
if(!mxIsNumeric(prhs[2]))
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","body must be numeric");
}
int body = (int) mxGetScalar(prhs[2])-1;
checkBodyOrFrameID(body, model);
if(mxGetM(prhs[3]) != 3 || mxGetM(prhs[4]) != 3 || mxGetN(prhs[3]) != 1 || mxGetN(prhs[4]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","lb and ub should both be 3x1 double vectors");
}
Vector3d lb;
Vector3d ub;
memcpy(lb.data(),mxGetPr(prhs[3]),sizeof(double)*3);
memcpy(ub.data(),mxGetPr(prhs[4]),sizeof(double)*3);
cnst = new WorldEulerConstraint(model,body,lb,ub,tspan);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst,"deleteRigidBodyConstraintmex","WorldEulerConstraint");
}
break;
// WorldGazeDirConstraint
case RigidBodyConstraint::WorldGazeDirConstraintType:
{
if(nrhs != 7 && nrhs != 6)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Usage ptr = constructPtrRigidBodyConstraintmex(RigidBodyConstraint::WorldGazeDirConstraintType,robot.mex_model_ptr,body,axis,dir,conethreshold,tspan)");
}
RigidBodyManipulator* model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
WorldGazeDirConstraint* cnst = nullptr;
Vector2d tspan;
if(nrhs == 6)
{
tspan<< -mxGetInf(), mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[6],tspan);
}
if(!mxIsNumeric(prhs[2]))
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","body must be numeric");
}
int body = (int) mxGetScalar(prhs[2])-1;
checkBodyOrFrameID(body, model);
Vector3d axis;
rigidBodyConstraintParse3dUnitVector(prhs[3], axis);
Vector3d dir;
rigidBodyConstraintParse3dUnitVector(prhs[4],dir);
double conethreshold = rigidBodyConstraintParseGazeConethreshold(prhs[5]);
cnst = new WorldGazeDirConstraint(model,body,axis,dir,conethreshold,tspan);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst, "deleteRigidBodyConstraintmex","WorldGazeDirConstraint");
}
break;
// WorldGazeOrientConstraint
case RigidBodyConstraint::WorldGazeOrientConstraintType:
{
if(nrhs != 7 && nrhs != 8)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Usage ptr = constructPtrRigidBodyConstraintmex(RigidBodyConstraint::WorldGazeOrientConstraintType, robot.mex_model_ptr,body,axis,quat_des,conethreshold,threshold,tspan)");
}
RigidBodyManipulator* model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
WorldGazeOrientConstraint* cnst = nullptr;
Vector2d tspan;
if(nrhs == 7)
{
tspan<< -mxGetInf(), mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[7],tspan);
}
if(!mxIsNumeric(prhs[2]))
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","body must be numeric");
}
int body = (int) mxGetScalar(prhs[2])-1;
checkBodyOrFrameID(body, model);
Vector3d axis;
rigidBodyConstraintParse3dUnitVector(prhs[3],axis);
Vector4d quat_des;
rigidBodyConstraintParseQuat(prhs[4],quat_des);
double conethreshold = rigidBodyConstraintParseGazeConethreshold(prhs[5]);
double threshold = rigidBodyConstraintParseGazeThreshold(prhs[6]);
cnst = new WorldGazeOrientConstraint(model,body,axis,quat_des,conethreshold,threshold,tspan);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst, "deleteRigidBodyConstraintmex","WorldGazeOrientConstraint");
}
break;
// WorldGazeTargetConstraint
case RigidBodyConstraint::WorldGazeTargetConstraintType:
{
if(nrhs != 8 && nrhs != 7)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Usage ptr = constructPtrRigidBodyConstraintmex(RigidBodyConstraint::WorldGazeTargetConstraintType,robot.mex_model_ptr,body,axis,target,gaze_origin,conethreshold,tspan)");
}
RigidBodyManipulator* model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
WorldGazeTargetConstraint* cnst = nullptr;
Vector2d tspan;
if(nrhs == 7)
{
tspan<< -mxGetInf(),mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[7],tspan);
}
if(!mxIsNumeric(prhs[2]))
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","body must be numeric");
}
int body = (int) mxGetScalar(prhs[2])-1;
checkBodyOrFrameID(body, model);
Vector3d axis;
rigidBodyConstraintParse3dUnitVector(prhs[3],axis);
Vector3d target;
assert(mxIsNumeric(prhs[4]));
assert(mxGetM(prhs[4]) == 3 &&mxGetN(prhs[4]) == 1);
memcpy(target.data(),mxGetPr(prhs[4]),sizeof(double)*3);
Vector4d gaze_origin;
Vector3d gaze_origin_tmp;
assert(mxIsNumeric(prhs[5]));
assert(mxGetM(prhs[5]) == 3 && mxGetN(prhs[5]) == 1);
memcpy(gaze_origin_tmp.data(),mxGetPr(prhs[5]),sizeof(double)*3);
gaze_origin.head(3) = gaze_origin_tmp;
gaze_origin(3) = 1.0;
double conethreshold = rigidBodyConstraintParseGazeConethreshold(prhs[6]);
cnst = new WorldGazeTargetConstraint(model,body,axis,target,gaze_origin,conethreshold,tspan);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst, "deleteRigidBodyConstraintmex","WorldGazeTargetConstraint");
}
break;
// RelativeGazeTargetConstraint
case RigidBodyConstraint::RelativeGazeTargetConstraintType:
{
if(nrhs != 7 && nrhs != 8 && nrhs != 9)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Usage ptr = constructPtrRigidBodyConstraintmex(RigidBodyConstraint::RelativeGazeTargetConstraintType, robot.mex_model_ptr,bodyA,bodyB,axis,target,gaze_origin,conethreshold,tspan)");
}
RigidBodyManipulator* model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
Vector2d tspan;
if(nrhs < 9)
{
tspan<<-mxGetInf(),mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[8],tspan);
}
if(!mxIsNumeric(prhs[2]) || !mxIsNumeric(prhs[3]) || mxGetNumberOfElements(prhs[2]) != 1 || mxGetNumberOfElements(prhs[3]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","bodyA and bodyB should be numeric scalars");
}
int bodyA_idx = (int) mxGetScalar(prhs[2])-1;
int bodyB_idx = (int) mxGetScalar(prhs[3])-1;
checkBodyOrFrameID(bodyA_idx, model,"bodyA");
checkBodyOrFrameID(bodyB_idx, model,"bodyB");
if(!mxIsNumeric(prhs[4]) || mxGetM(prhs[4]) != 3 || mxGetN(prhs[4]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","axis should be 3x1 vector");
}
Vector3d axis;
memcpy(axis.data(),mxGetPr(prhs[4]),sizeof(double)*3);
double axis_norm = axis.norm();
if(axis_norm<1e-10)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","axis should be a nonzero vector");
}
axis = axis/axis_norm;
if(!mxIsNumeric(prhs[5]) || mxGetM(prhs[5]) != 3 || mxGetN(prhs[5]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","target should be 3x1 vector");
}
Vector3d target;
memcpy(target.data(),mxGetPr(prhs[5]),sizeof(double)*3);
if(!mxIsNumeric(prhs[6]) || mxGetM(prhs[6]) != 3 || mxGetN(prhs[6]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","gaze_origin should be 3x1 vector");
}
Vector4d gaze_origin;
memcpy(gaze_origin.data(),mxGetPr(prhs[6]),sizeof(double)*3);
gaze_origin(3) = 1.0;
double conethreshold;
if(nrhs<8)
{
conethreshold = 0.0;
}
else
{
if(!mxIsNumeric(prhs[7]) || mxGetNumberOfElements(prhs[7]) != 1)
{
if(mxGetNumberOfElements(prhs[7]) == 0)
{
conethreshold = 0.0;
}
else
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","conethreshold should be a double scalar");
}
}
conethreshold = mxGetScalar(prhs[7]);
if(conethreshold<0)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","conethreshold should be nonnegative");
}
}
RelativeGazeTargetConstraint* cnst = new RelativeGazeTargetConstraint(model,bodyA_idx, bodyB_idx,axis,target,gaze_origin,conethreshold,tspan);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst, "deleteRigidBodyConstraintmex","RelativeGazeTargetConstraint");
}
break;
// RelativeGazeDirConstraint
case RigidBodyConstraint::RelativeGazeDirConstraintType:
{
if(nrhs != 6 && nrhs != 7 && nrhs != 8)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Usage ptr = constructPtrRigidBodyConstraintmex(RigidBodyConstraint::RelativeGazeDirConstraintType, robot.mex_model_ptr,bodyA,bodyB,axis,dir,conethreshold,tspan)");
}
RigidBodyManipulator* model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
Vector2d tspan;
if(nrhs < 8)
{
tspan<<-mxGetInf(),mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[7],tspan);
}
if(!mxIsNumeric(prhs[2]) || !mxIsNumeric(prhs[3]) || mxGetNumberOfElements(prhs[2]) != 1 || mxGetNumberOfElements(prhs[3]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","bodyA and bodyB should be numeric scalars");
}
int bodyA_idx = (int) mxGetScalar(prhs[2])-1;
int bodyB_idx = (int) mxGetScalar(prhs[3])-1;
checkBodyOrFrameID(bodyA_idx, model,"bodyA");
checkBodyOrFrameID(bodyB_idx, model,"bodyB");
if(!mxIsNumeric(prhs[4]) || mxGetM(prhs[4]) != 3 || mxGetN(prhs[4]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","axis should be 3x1 vector");
}
Vector3d axis;
memcpy(axis.data(),mxGetPr(prhs[4]),sizeof(double)*3);
double axis_norm = axis.norm();
if(axis_norm<1e-10)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","axis should be a nonzero vector");
}
axis = axis/axis_norm;
if(!mxIsNumeric(prhs[5]) || mxGetM(prhs[5]) != 3 || mxGetN(prhs[5]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","dir should be 3x1 vector");
}
Vector3d dir;
memcpy(dir.data(),mxGetPr(prhs[5]),sizeof(double)*3);
double conethreshold;
if(nrhs<7)
{
conethreshold = 0.0;
}
else
{
if(!mxIsNumeric(prhs[6]) || mxGetNumberOfElements(prhs[6]) != 1)
{
if(mxGetNumberOfElements(prhs[6]) == 0)
{
conethreshold = 0.0;
}
else
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","conethreshold should be a double scalar");
}
}
conethreshold = mxGetScalar(prhs[6]);
if(conethreshold<0)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","conethreshold should be nonnegative");
}
}
RelativeGazeDirConstraint* cnst = new RelativeGazeDirConstraint(model,bodyA_idx, bodyB_idx,axis,dir,conethreshold,tspan);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst, "deleteRigidBodyConstraintmex","RelativeGazeDirConstraint");
}
break;
// WorldCoMConstraint
case RigidBodyConstraint::WorldCoMConstraintType:
{
if(nrhs != 4 && nrhs != 5 && nrhs != 6)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Usage ptr = constructPtrRigidBodyConstraintmex(RigidBodyConstraint::WorldCoMConstraintType,robot.mex_model_ptr,lb,ub,tspan,robotnum)");
}
RigidBodyManipulator* model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
WorldCoMConstraint* cnst = nullptr;
Vector2d tspan;
int* robotnum;
int num_robot;
if(nrhs <= 5)
{
num_robot = 1;
robotnum = new int[num_robot];
robotnum[0] = 0;
}
else
{
num_robot = mxGetNumberOfElements(prhs[5]);
double* robotnum_tmp = new double[num_robot];
robotnum = new int[num_robot];
memcpy(robotnum_tmp,mxGetPr(prhs[5]),sizeof(double)*num_robot);
for(int i = 0;i<num_robot;i++)
{
robotnum[i] = (int) robotnum_tmp[i]-1;
}
delete[] robotnum_tmp;
}
if(nrhs<=4)
{
tspan<<-mxGetInf(),mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[4],tspan);
}
set<int> robotnumset(robotnum,robotnum+num_robot);
int n_pts = 1;
if(mxGetM(prhs[2]) != 3 || mxGetM(prhs[3]) != 3 || mxGetN(prhs[2]) != n_pts || mxGetN(prhs[3]) != n_pts || !mxIsNumeric(prhs[2]) || !mxIsNumeric(prhs[3]))
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","lb and ub should both be 3x1 double vectors");
}
Vector3d lb;
Vector3d ub;
memcpy(lb.data(),mxGetPr(prhs[2]),sizeof(double)*3);
memcpy(ub.data(),mxGetPr(prhs[3]),sizeof(double)*3);
cnst = new WorldCoMConstraint(model,lb,ub,tspan,robotnumset);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst,"deleteRigidBodyConstraintmex","WorldCoMConstraint");
delete[] robotnum;
}
break;
// WorldPositionConstraint
case RigidBodyConstraint::WorldPositionConstraintType:
{
if(nrhs!= 7 && nrhs != 6)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Usage ptr = constructPtrRigidBodyConstraintmex(RigidBodyConstraint::WorldPositionConstraintType, robot.mex_model_ptr,body,pts,lb,ub,tspan)");
}
RigidBodyManipulator* model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
WorldPositionConstraint* cnst = nullptr;
Vector2d tspan;
if(nrhs <= 6)
{
tspan<< -mxGetInf(), mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[6],tspan);
}
if(!mxIsNumeric(prhs[2]) || mxGetM(prhs[2])!= 1 || mxGetN(prhs[2]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","body must be numeric");
}
int body = (int) mxGetScalar(prhs[2])-1;
checkBodyOrFrameID(body, model);
int n_pts = mxGetN(prhs[3]);
if(!mxIsNumeric(prhs[3])||mxGetM(prhs[3]) != 3)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Argument 3 should be a double matrix with 3 rows");
}
MatrixXd pts_tmp(3,n_pts);
memcpy(pts_tmp.data(),mxGetPr(prhs[3]),sizeof(double)*3*n_pts);
MatrixXd pts(4,n_pts);
pts.block(0,0,3,n_pts) = pts_tmp;
pts.block(3,0,1,n_pts) = MatrixXd::Ones(1,n_pts);
MatrixXd lb(3,n_pts);
MatrixXd ub(3,n_pts);
if(mxGetM(prhs[4]) != 3 || mxGetN(prhs[4]) != n_pts || mxGetM(prhs[5]) != 3 || mxGetN(prhs[5]) != n_pts)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","lb and ub should both be 3xn_pts double matrix");
}
memcpy(lb.data(),mxGetPr(prhs[4]),sizeof(double)*3*n_pts);
memcpy(ub.data(),mxGetPr(prhs[5]),sizeof(double)*3*n_pts);
cnst = new WorldPositionConstraint(model,body,pts,lb,ub,tspan);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst,"deleteRigidBodyConstraintmex","WorldPositionConstraint");
}
break;
// WorldPositionInFrameConstraint
case RigidBodyConstraint::WorldPositionInFrameConstraintType:
{
if(nrhs!= 8 && nrhs != 7)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Usage ptr = constructPtrRigidBodyConstraintmex(RigidBodyConstraint::WorldPositionInFrameConstraint, robot.mex_model_ptr,body,pts,lb,ub,tspan)");
}
RigidBodyManipulator* model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
WorldPositionInFrameConstraint* cnst = nullptr;
Vector2d tspan;
if(nrhs <= 7)
{
tspan<< -mxGetInf(), mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[7],tspan);
}
int body = (int) mxGetScalar(prhs[2])-1;
checkBodyOrFrameID(body, model);
int n_pts = mxGetN(prhs[3]);
if(!mxIsNumeric(prhs[3])||mxGetM(prhs[3]) != 3)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Argument 3 should be a double matrix with 3 rows");
}
MatrixXd pts_tmp(3,n_pts);
memcpy(pts_tmp.data(),mxGetPr(prhs[3]),sizeof(double)*3*n_pts);
MatrixXd pts(4,n_pts);
pts.block(0,0,3,n_pts) = pts_tmp;
pts.block(3,0,1,n_pts) = MatrixXd::Ones(1,n_pts);
if(!mxIsNumeric(prhs[4]) || mxGetM(prhs[4]) != 4 || mxGetN(prhs[4]) != 4)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Argument 4 should be a 4x4 double matrix");
}
Matrix4d T_world_to_frame;
memcpy(T_world_to_frame.data(),mxGetPr(prhs[4]),sizeof(double)*16);
MatrixXd lb(3,n_pts);
MatrixXd ub(3,n_pts);
if(mxGetM(prhs[5]) != 3 || mxGetN(prhs[5]) != n_pts || mxGetM(prhs[6]) != 3 || mxGetN(prhs[6]) != n_pts)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","lb and ub should both be 3xn_pts double matrix");
}
memcpy(lb.data(),mxGetPr(prhs[5]),sizeof(double)*3*n_pts);
memcpy(ub.data(),mxGetPr(prhs[6]),sizeof(double)*3*n_pts);
cnst = new WorldPositionInFrameConstraint(model,body,pts,T_world_to_frame,
lb,ub,tspan);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst,"deleteRigidBodyConstraintmex","WorldPositionInFrameConstraint");
}
break;
// WorldQuatConstraint
case RigidBodyConstraint::WorldQuatConstraintType:
{
if(nrhs!= 6 && nrhs!= 5)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Usage ptr = constructPtrRigidBodyConstraintmex(RigidBodyConstraint::WorldQuatConstraintType,robot.mex_model_ptr,body,quat_des,tol,tspan)");
}
RigidBodyManipulator* model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
WorldQuatConstraint* cnst = nullptr;
Vector2d tspan;
if(nrhs <= 5)
{
tspan<<-mxGetInf(),mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[5],tspan);
}
int body = (int) mxGetScalar(prhs[2])-1;
checkBodyOrFrameID(body, model);
Vector4d quat_des;
rigidBodyConstraintParseQuat(prhs[3],quat_des);
double tol = mxGetScalar(prhs[4]);
cnst = new WorldQuatConstraint(model,body,quat_des,tol,tspan);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst,"deleteRigidBodyConstraintmex","WorldQuatConstraint");
}
break;
// Point2PointDistanceConstraint
case RigidBodyConstraint::Point2PointDistanceConstraintType:
{
if(nrhs != 8 && nrhs != 9)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Usage ptr = constructRigidBodyConstraintmex(RigidBodyConstraint::Point2PointDistanceConstraintType, robot.mex_model_ptr,bodyA,bodyB,ptA,ptB,dist_lb,dist_ub,tspan");
}
RigidBodyManipulator *model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
Vector2d tspan;
if(nrhs <= 8)
{
tspan<< -mxGetInf(), mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[8],tspan);
}
if(!mxIsNumeric(prhs[2]) || !mxIsNumeric(prhs[3]))
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","bodyA and bodyB must be numeric");
}
if(mxGetNumberOfElements(prhs[2]) != 1 || mxGetNumberOfElements(prhs[3]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","bodyA and bodyB must be a scalar");
}
int bodyA = (int) mxGetScalar(prhs[2])-1;
int bodyB = (int) mxGetScalar(prhs[3])-1;
checkBodyOrFrameID(bodyA, model,"bodyA");
checkBodyOrFrameID(bodyB, model,"bodyB");
if(bodyA == bodyB)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","bodyA and bodyB should be different");
}
if(!mxIsNumeric(prhs[4]) || !mxIsNumeric(prhs[5]))
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","ptA and ptB should be numeric");
}
int npts = mxGetN(prhs[4]);
if(mxGetM(prhs[4]) != 3 || mxGetM(prhs[5]) != 3)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","ptA and ptB should have 3 rows");
}
if(mxGetN(prhs[5]) != npts)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","ptA and ptB should have the same number of columns");
}
MatrixXd ptA_tmp(3,npts);
MatrixXd ptB_tmp(3,npts);
memcpy(ptA_tmp.data(),mxGetPr(prhs[4]),sizeof(double)*3*npts);
memcpy(ptB_tmp.data(),mxGetPr(prhs[5]),sizeof(double)*3*npts);
MatrixXd ptA(4,npts);
MatrixXd ptB(4,npts);
ptA.block(0,0,3,npts) = ptA_tmp;
ptB.block(0,0,3,npts) = ptB_tmp;
ptA.row(3) = MatrixXd::Ones(1,npts);
ptB.row(3) = MatrixXd::Ones(1,npts);
if(!mxIsNumeric(prhs[6]) || !mxIsNumeric(prhs[7]) || mxGetM(prhs[6]) != 1 || mxGetM(prhs[7]) != 1 || mxGetN(prhs[6]) != npts || mxGetN(prhs[7]) != npts)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","dist_lb and dist_ub must be 1 x npts numeric");
}
VectorXd dist_lb(npts);
VectorXd dist_ub(npts);
memcpy(dist_lb.data(),mxGetPr(prhs[6]),sizeof(double)*npts);
memcpy(dist_ub.data(),mxGetPr(prhs[7]),sizeof(double)*npts);
for(int i = 0;i<npts;i++)
{
if(dist_lb(i)>dist_ub(i))
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","dist_lb must be no larger than dist_ub");
}
}
Point2PointDistanceConstraint* cnst = new Point2PointDistanceConstraint(model,bodyA,bodyB,ptA,ptB,dist_lb,dist_ub,tspan);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst,"deleteRigidBodyConstraintmex","Point2PointDistanceConstraint");
}
break;
// Point2LineSegDistConstraint
case RigidBodyConstraint::Point2LineSegDistConstraintType:
{
if(nrhs != 8 && nrhs != 9)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Usage ptr = constructPtrRigidBodyConstraintmex(RigidBodyConstraint::Point2LineSegDistConstraint, robot.mex_model_ptr,pt_body,pt,line_body,line_ends,lb,ub,tspan");
}
RigidBodyManipulator *model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
Vector2d tspan;
if(nrhs <= 8)
{
tspan<< -mxGetInf(), mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[8],tspan);
}
if(!mxIsNumeric(prhs[2])||mxGetNumberOfElements(prhs[2]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","pt_body should be a numeric scalar");
}
int pt_body = (int) mxGetScalar(prhs[2])-1;
if(pt_body>=model->num_bodies || pt_body<0)
{
mexErrMsgIdAndTxt("Drake:constructPtrPoint2LineSegDistConstraintmex:BadInputs","pt_body is invalid");
}
if(!mxIsNumeric(prhs[3]) || mxGetM(prhs[3]) != 3 || mxGetN(prhs[3]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","pt should be a 3x1 vector");
}
Vector4d pt;
memcpy(pt.data(),mxGetPr(prhs[3]),sizeof(double)*3);
pt(3) = 1.0;
if(!mxIsNumeric(prhs[4])||mxGetNumberOfElements(prhs[4]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","line_body should be a numeric scalar");
}
int line_body = (int) mxGetScalar(prhs[4])-1;
if(line_body>=model->num_bodies || line_body<0)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","line_body is invalid");
}
if(!mxIsNumeric(prhs[5]) || mxGetM(prhs[5]) != 3 || mxGetN(prhs[5]) != 2)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","line_ends should be a 3x2 vector");
}
Matrix<double,4,2> line_ends;
Matrix<double,3,2> line_ends_tmp;
memcpy(line_ends_tmp.data(),mxGetPr(prhs[5]),sizeof(double)*6);
line_ends.block(0,0,3,2) = line_ends_tmp;
line_ends.row(3) = MatrixXd::Ones(1,2);
if(!mxIsNumeric(prhs[6]) || !mxIsNumeric(prhs[7]) || mxGetNumberOfElements(prhs[6]) != 1 || mxGetNumberOfElements(prhs[7]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","dist_lb, dist_ub should be scalars");
}
double dist_lb = mxGetScalar(prhs[6]);
double dist_ub = mxGetScalar(prhs[7]);
if(dist_lb<0 || dist_lb>dist_ub)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","dist_lb should be nonnegative, and dist_ub should be no less than dist_lb");
}
Point2LineSegDistConstraint* cnst = new Point2LineSegDistConstraint(model,pt_body,pt,line_body,line_ends,dist_lb,dist_ub,tspan);
plhs[0] = createDrakeConstraintMexPointer((void*) cnst,"deleteRigidBodyConstraintmex","Point2LineSegDistConstraint");
}
break;
// WorldFixedPositionConstraint
case RigidBodyConstraint::WorldFixedPositionConstraintType:
{
if(nrhs != 5 && nrhs != 4)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs", "Usage ptr = constructPtrRigidBodyConstraintmex(RigidBodyConstraint::WorldFixedPositionConstraintType, robot.mex_model_ptr,body,pts,tspan)");
}
RigidBodyManipulator *model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
WorldFixedPositionConstraint* cnst = nullptr;
Vector2d tspan;
if(nrhs <= 4)
{
tspan<< -mxGetInf(),mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[4],tspan);
}
int body = (int) mxGetScalar(prhs[2])-1;
int n_pts = mxGetN(prhs[3]);
if(!mxIsNumeric(prhs[3])||mxGetM(prhs[3]) != 3)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Argument 4 should be a double matrix with 3 rows");
}
MatrixXd pts_tmp(3,n_pts);
memcpy(pts_tmp.data(),mxGetPr(prhs[3]),sizeof(double)*3*n_pts);
MatrixXd pts(4,n_pts);
pts.block(0,0,3,n_pts) = pts_tmp;
pts.block(3,0,1,n_pts) = MatrixXd::Ones(1,n_pts);
cnst = new WorldFixedPositionConstraint(model,body,pts,tspan);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst,"deleteRigidBodyConstraintmex","WorldFixedPositionConstraint");
}
break;
// WorldFixedOrientConstraint
case RigidBodyConstraint::WorldFixedOrientConstraintType:
{
if(nrhs != 4 && nrhs != 3)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs", "Usage ptr = constructPtrRigidBodyConstraintmex(RigidBodyConstraint::WorldFixedOrientConstraintType,robot.mex_model_ptr,body,tspan)");
}
RigidBodyManipulator *model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
WorldFixedOrientConstraint* cnst = nullptr;
Vector2d tspan;
if(nrhs <= 3)
{
tspan<< -mxGetInf(),mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[3],tspan);
}
int body = (int) mxGetScalar(prhs[2])-1;
cnst = new WorldFixedOrientConstraint(model,body,tspan);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst,"deleteRigidBodyConstraintmex","WorldFixedOrientConstraint");
}
break;
// WorldFixedBodyPoseConstraint
case RigidBodyConstraint::WorldFixedBodyPoseConstraintType:
{
if(nrhs != 4 && nrhs != 3)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs", "Usage ptr = constructPtrRigidBodyConstraintmex(RigidBodyConstraint::WorldFixedBodyPoseConstraintType,robot.mex_model_ptr,body,tspan)");
}
RigidBodyManipulator *model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
WorldFixedBodyPoseConstraint* cnst = nullptr;
Vector2d tspan;
if(nrhs <= 3)
{
tspan<< -mxGetInf(),mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[3],tspan);
}
int body = (int) mxGetScalar(prhs[2])-1;
cnst = new WorldFixedBodyPoseConstraint(model,body,tspan);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst,"deleteRigidBodyConstraintmex","WorldFixedBodyPoseConstraint");
}
break;
// PostureChangeConstraintType
case RigidBodyConstraint::PostureChangeConstraintType:
{
if(nrhs != 6 && nrhs != 5)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Usage ptr = constructPtrRigidBodyConstraintmex(RigidBodyConstraint::PostureChangeConstraintType,robot.mex_model_ptr,joint_ind,lb_change,ub_change,tspan");
}
RigidBodyManipulator* model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
Vector2d tspan;
if(nrhs <= 5)
{
tspan<<-mxGetInf(),mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[5],tspan);
}
if(!mxIsNumeric(prhs[2]) || mxGetN(prhs[2]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","joint_ind must be a column numeric vector");
}
int num_joints = mxGetM(prhs[2]);
VectorXd joint_ind_tmp(num_joints);
memcpy(joint_ind_tmp.data(),mxGetPr(prhs[2]),sizeof(double)*num_joints);
VectorXi joint_ind(num_joints);
for(int i = 0;i<num_joints;i++)
{
joint_ind(i) = (int) joint_ind_tmp(i)-1;
if(joint_ind(i)<0 || joint_ind(i)>=model->num_dof)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","joint_ind must be within [1,nq]");
}
}
if(!mxIsNumeric(prhs[3]) || mxGetM(prhs[3]) != num_joints || mxGetN(prhs[3]) != 1 ||!mxIsNumeric(prhs[4]) || mxGetM(prhs[4]) != num_joints || mxGetN(prhs[4]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","lb_change and upper bound change must be both numeric vector, with the same size as joint_ind");
}
VectorXd lb_change(num_joints);
VectorXd ub_change(num_joints);
memcpy(lb_change.data(),mxGetPr(prhs[3]),sizeof(double)*num_joints);
memcpy(ub_change.data(),mxGetPr(prhs[4]),sizeof(double)*num_joints);
for(int i = 0;i<num_joints;i++)
{
double lb_change_min = model->joint_limit_min[joint_ind(i)]-model->joint_limit_max[joint_ind(i)];
double ub_change_max = model->joint_limit_max[joint_ind(i)]-model->joint_limit_min[joint_ind(i)];
lb_change(i) = (lb_change_min<lb_change(i)?lb_change(i):lb_change_min);
ub_change(i) = (ub_change_max>ub_change(i)?ub_change(i):ub_change_max);
if(lb_change(i)>ub_change(i))
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","lb_change must be no larger than ub_change");
}
}
PostureChangeConstraint* cnst = new PostureChangeConstraint(model,joint_ind,lb_change,ub_change,tspan);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst,"deleteRigidBodyConstraintmex","PostureChangeConstraint");
}
break;
case RigidBodyConstraint::RelativePositionConstraintType:
{
if(nrhs != 9 && nrhs != 8)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Usage ptr = constructPtrRigidBodyConstraintmex(RigidBodyConstraint::RelativePositionConstraintType,robot.mex_model_ptr,pts,lb,ub,bodyA_idx,bodyB_idx,bTbp,tspan");
}
RigidBodyManipulator* model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
Vector2d tspan;
if(nrhs <= 8)
{
tspan<<-mxGetInf(),mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[8],tspan);
}
if(!mxIsNumeric(prhs[2])|| mxGetM(prhs[2]) != 3)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Argument 3 (pts) should be a double matrix with 3 rows");
}
int n_pts = mxGetN(prhs[3]);
MatrixXd pts_tmp(3,n_pts);
memcpy(pts_tmp.data(),mxGetPr(prhs[2]),sizeof(double)*3*n_pts);
MatrixXd pts(4,n_pts);
pts.block(0,0,3,n_pts) = pts_tmp;
pts.block(3,0,1,n_pts) = MatrixXd::Ones(1,n_pts);
MatrixXd lb(3,n_pts);
MatrixXd ub(3,n_pts);
if(!mxIsNumeric(prhs[3]) || mxGetM(prhs[3]) != 3 || mxGetN(prhs[3]) != n_pts || !mxIsNumeric(prhs[4]) || mxGetM(prhs[4]) != 3 || mxGetN(prhs[4]) != n_pts)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","lb and ub should both be 3xn_pts double matrix");
}
memcpy(lb.data(),mxGetPr(prhs[3]),sizeof(double)*3*n_pts);
memcpy(ub.data(),mxGetPr(prhs[4]),sizeof(double)*3*n_pts);
if(!mxIsNumeric(prhs[5]) || mxGetM(prhs[5]) != 1 || mxGetN(prhs[5]) != 1 || !mxIsNumeric(prhs[6]) || mxGetM(prhs[6]) != 1 || mxGetN(prhs[6]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","bodyA_idx and bodyB_idx should be numeric scalars");
}
int bodyA_idx = static_cast<int>(mxGetScalar(prhs[5])-1);
int bodyB_idx = static_cast<int>(mxGetScalar(prhs[6])-1);
checkBodyOrFrameID(bodyA_idx,model,"bodyA");
checkBodyOrFrameID(bodyB_idx,model,"bodyB");
Matrix<double,7,1> bTbp;
if(!mxIsNumeric(prhs[7]) || mxGetM(prhs[7]) != 7 || mxGetN(prhs[7]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","bTbp should be 7x1 numeric vector");
}
memcpy(bTbp.data(),mxGetPr(prhs[7]),sizeof(double)*7);
double quat_norm = bTbp.block(3,0,4,1).norm();
if(abs(quat_norm-1)>1e-5)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","bTbp(4:7) should be a unit quaternion");
}
bTbp.block(3,0,4,1) /= quat_norm;
RelativePositionConstraint* cnst = new RelativePositionConstraint(model,pts,lb,ub,bodyA_idx,bodyB_idx,bTbp,tspan);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst,"deleteRigidBodyConstraintmex","RelativePositionConstraint");
}
break;
case RigidBodyConstraint::RelativeQuatConstraintType:
{
if(nrhs != 7 && nrhs != 6)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Usage ptr = constructPtrRigidBodyConstraintmex(RigidBodyConstraint::RelativeQuatConstraintType,robot.mex_model_ptr,bodyA_idx,bodyB_idx,quat_des,tol,tspan");
}
RigidBodyManipulator* model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
Vector2d tspan;
if(nrhs <= 6)
{
tspan<<-mxGetInf(),mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[6],tspan);
}
if(!mxIsNumeric(prhs[2])|| mxGetM(prhs[2]) != 1 || mxGetN(prhs[2]) != 1 || !mxIsNumeric(prhs[3])|| mxGetM(prhs[3]) != 1 || mxGetN(prhs[3]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Argument 3 (bodyA_idx) and 4 (bodyB_idx) should be both scalars");
}
int bodyA_idx = static_cast<int>(mxGetScalar(prhs[2]))-1;
int bodyB_idx = static_cast<int>(mxGetScalar(prhs[3]))-1;
checkBodyOrFrameID(bodyA_idx,model,"bodyA");
checkBodyOrFrameID(bodyB_idx,model,"bodyB");
if(!mxIsNumeric(prhs[4]) || mxGetM(prhs[4]) != 4 || mxGetN(prhs[4]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Argument 5 (quat_des) should be 4 x 1 double vector");
}
Vector4d quat_des;
memcpy(quat_des.data(),mxGetPr(prhs[4]),sizeof(double)*4);
double quat_norm = quat_des.norm();
if(abs(quat_norm-1.0)>1e-5)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Argument 5 should be a quaternion with unit length");
}
if(!mxIsNumeric(prhs[5]) || mxGetM(prhs[5]) != 1 || mxGetN(prhs[5]) != 1)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Argument 6 (tol) should be a double scalar");
}
double tol = mxGetScalar(prhs[5]);
RelativeQuatConstraint* cnst = new RelativeQuatConstraint(model,bodyA_idx,bodyB_idx,quat_des,tol,tspan);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst,"deleteRigidBodyConstraintmex","RelativeQuatConstraint");
}
break;
case RigidBodyConstraint::MinDistanceConstraintType:
{
if(nrhs != 4 && nrhs != 5)
{
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs",
"Usage ptr = constructPtrRigidBodyConstraintmex(RigidBodyConstraint::MinDistanceConstraintType, robot.mex_model_ptr,min_distance,active_collision_options,tspan)");
}
RigidBodyManipulator* model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[1]);
Vector2d tspan;
if(nrhs == 4)
{
tspan<< -mxGetInf(), mxGetInf();
}
else
{
rigidBodyConstraintParseTspan(prhs[4],tspan);
}
double min_distance = (double) mxGetScalar(prhs[2]);
// Parse `active_collision_options`
vector<int> active_bodies_idx;
set<string> active_group_names;
// First get the list of body indices for which to compute distances
const mxArray* active_collision_options = prhs[3];
const mxArray* body_idx = mxGetField(active_collision_options,0,"body_idx");
if (body_idx != NULL) {
//DEBUG
//cout << "collisionDetectmex: Received body_idx" << endl;
//END_DEBUG
int n_active_bodies = mxGetNumberOfElements(body_idx);
//DEBUG
//cout << "collisionDetectmex: n_active_bodies = " << n_active_bodies << endl;
//END_DEBUG
active_bodies_idx.resize(n_active_bodies);
memcpy(active_bodies_idx.data(),(int*) mxGetData(body_idx),
sizeof(int)*n_active_bodies);
transform(active_bodies_idx.begin(),active_bodies_idx.end(),
active_bodies_idx.begin(),
[](int i){return --i;});
}
// Then get the group names for which to compute distances
const mxArray* collision_groups = mxGetField(active_collision_options,0,
"collision_groups");
if (collision_groups != NULL) {
for (const string& str : get_strings(collision_groups)) {
active_group_names.insert(str);
}
}
auto cnst = new MinDistanceConstraint(model,min_distance,active_bodies_idx,active_group_names,tspan);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst,"deleteRigidBodyConstraintmex",
"MinDistanceConstraint");
}
break;
default:
mexErrMsgIdAndTxt("Drake:constructPtrRigidBodyConstraintmex:BadInputs","Unsupported constraint type");
break;
}
}
/** Render the sensor frustum.
*/
void
SensorFrustumGeometry::render(RenderContext& rc,
double currentTime) const
{
Material material;
material.setDiffuse(m_color);
material.setOpacity(m_opacity);
rc.setVertexInfo(VertexSpec::Position);
rc.bindMaterial(&material);
if (source() && target())
{
Vector3d p = target()->position(currentTime) - source()->position(currentTime);
// Get the position of the source in the local coordinate system of the target
Matrix3d targetRotation = target()->orientation(currentTime).conjugate().toRotationMatrix();
Vector3d p2 = targetRotation * -p;
// Special handling for ellipsoidal objects, i.e. planets.
// TODO: Should have a cleaner solution here than a dynamic_cast
bool ellipsoidalTarget = false;
Vector3d targetSemiAxes = Vector3d::Ones();
if (dynamic_cast<WorldGeometry*>(target()->geometry()))
{
ellipsoidalTarget = true;
targetSemiAxes = dynamic_cast<WorldGeometry*>(target()->geometry())->ellipsoidAxes().cast<double>() / 2.0;
}
Quaterniond rotation = source()->orientation(currentTime);
Matrix3d m = (rotation * m_orientation).toRotationMatrix();
double horizontalSize = tan(m_frustumHorizontalAngle / 2.0);
double verticalSize = tan(m_frustumVerticalAngle / 2.0);
bool showInside = false;
rc.pushModelView();
rc.rotateModelView(rotation.cast<float>().conjugate());
m_frustumPoints.clear();
const unsigned int sideDivisions = 12;
const unsigned int sections = 4 * sideDivisions;
for (unsigned int i = 0; i < sections; ++i)
{
Vector3d r;
if (frustumShape() == Elliptical)
{
double t = (double) i / (double) sections;
double theta = 2 * PI * t;
r = Vector3d(horizontalSize * cos(theta), verticalSize * sin(theta), 1.0).normalized();
}
else
{
if (i < sideDivisions)
{
double t = i / double(sideDivisions);
r = Vector3d((t - 0.5) * horizontalSize, -verticalSize * 0.5, 1.0).normalized();
}
else if (i < sideDivisions * 2)
{
double t = (i - sideDivisions) / double(sideDivisions);
r = Vector3d(horizontalSize * 0.5, (t - 0.5) * verticalSize, 1.0).normalized();
}
else if (i < sideDivisions * 3)
{
double t = (i - sideDivisions * 2) / double(sideDivisions);
r = Vector3d((0.5 - t) * horizontalSize, verticalSize * 0.5, 1.0).normalized();
}
else
{
double t = (i - sideDivisions * 3) / double(sideDivisions);
r = Vector3d(-horizontalSize * 0.5, (0.5 - t) * verticalSize, 1.0).normalized();
}
}
r = m * r;
double intersectDistance = m_range;
if (TestRayEllipsoidIntersection(p2, targetRotation * r, targetSemiAxes, &intersectDistance))
{
// Reduce the intersect distance slightly to reduce depth precision problems
// when drawing the sensor footprint on a planet surface.
intersectDistance *= 0.9999;
}
m_frustumPoints.push_back(r * min(m_range, intersectDistance));
}
if (m_opacity > 0.0f)
{
// Draw the frustum
material.setOpacity(m_opacity);
rc.bindMaterial(&material);
if (showInside)
{
glDisable(GL_CULL_FACE);
}
glBegin(GL_TRIANGLE_FAN);
glVertex3d(0.0, 0.0, 0.0);
for (int i = (int) m_frustumPoints.size() - 1; i >= 0; --i)
{
glVertex3dv(m_frustumPoints[i].data());
}
glVertex3dv(m_frustumPoints.back().data());
glEnd();
glEnable(GL_CULL_FACE);
}
if (m_footprintOpacity > 0.0f)
{
material.setOpacity(1.0f);
rc.bindMaterial(&material);
glBegin(GL_LINE_STRIP);
for (unsigned int i = 0; i < m_frustumPoints.size(); ++i)
{
glVertex3dv(m_frustumPoints[i].data());
}
glVertex3dv(m_frustumPoints[0].data());
glEnd();
}
if (m_gridOpacity > 0.0f)
{
// Draw grid lines
unsigned int ringCount = 8;
unsigned int rayCount = frustumShape() == Rectangular ? 4 : 8;
material.setOpacity(m_gridOpacity);
rc.bindMaterial(&material);
for (unsigned int i = 1; i < ringCount; ++i)
{
double t = (double) i / (double) ringCount;
glBegin(GL_LINE_LOOP);
for (unsigned int j = 0; j < m_frustumPoints.size(); ++j)
{
Vector3d v = m_frustumPoints[j] * t;
glVertex3dv(v.data());
}
glEnd();
}
unsigned int rayStep = sections / rayCount;
glBegin(GL_LINES);
for (unsigned int i = 0; i < sections; i += rayStep)
{
glVertex3d(0.0, 0.0, 0.0);
glVertex3dv(m_frustumPoints[i].data());
}
glEnd();
}
rc.popModelView();
}
}
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<RigidBodyTree*>(prhs[0]);
return;
}
const mxArray* pRBM = prhs[0];
RigidBodyTree* model = new RigidBodyTree();
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
std::unique_ptr<RigidBody> b(new RigidBody());
b->set_body_index(i);
b->set_name(mxGetStdString(mxGetPropertySafe(pBodies, i, "linkname")));
pm = mxGetPropertySafe(pBodies, i, "robotnum");
b->set_model_instance_id((int)mxGetScalar(pm) - 1);
pm = mxGetPropertySafe(pBodies, i, "mass");
b->set_mass(mxGetScalar(pm));
pm = mxGetPropertySafe(pBodies, i, "com");
Eigen::Vector3d com;
if (!mxIsEmpty(pm)) {
memcpy(com.data(), mxGetPrSafe(pm), sizeof(double) * 3);
b->set_center_of_mass(com);
}
pm = mxGetPropertySafe(pBodies, i, "I");
if (!mxIsEmpty(pm)) {
drake::SquareTwistMatrix<double> I;
memcpy(I.data(), mxGetPrSafe(pm), sizeof(double) * 6 * 6);
b->set_spatial_inertia(I);
}
pm = mxGetPropertySafe(pBodies, i, "position_num");
b->set_position_start_index((int)mxGetScalar(pm) - 1); // zero-indexed
pm = mxGetPropertySafe(pBodies, i, "velocity_num");
b->set_velocity_start_index((int)mxGetScalar(pm) - 1); // zero-indexed
pm = mxGetPropertySafe(pBodies, i, "parent");
if (!pm || mxIsEmpty(pm)) {
b->set_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->set_parent(model->bodies[parent_ind].get());
}
if (b->has_parent_body()) {
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(std::move(joint));
}
// DEBUG
// mexPrintf("constructModelmex: About to parse collision geometry\n");
// END_DEBUG
pm = mxGetPropertySafe(pBodies, i, "collision_geometry");
Isometry3d 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;
RigidBodyCollisionElement element(T, b.get());
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 RigidBodyTree::compile, which is called below.
// if (!model->bodies[i]->has_parent_body()) {
// 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(std::move(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]->set_contact_points(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(new RigidBodyFrame());
fr->set_name(mxGetStdString(mxGetPropertySafe(pFrames, i, "name")));
pm = mxGetPropertySafe(pFrames, i, "body_ind");
fr->set_rigid_body(model->bodies[(int)mxGetScalar(pm) - 1].get());
pm = mxGetPropertySafe(pFrames, i, "T");
memcpy(fr->get_mutable_transform_to_body()->data(), mxGetPrSafe(pm),
sizeof(double) * 4 * 4);
fr->set_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");
size_t joint_index = static_cast<size_t>(mxGetScalar(pm) - 1);
double reduction =
mxGetScalar(mxGetPropertySafe(pActuators, i, "reduction"));
double effort_min =
mxGetScalar(mxGetPropertySafe(pBodies, joint_index, "effort_min"));
double effort_max =
mxGetScalar(mxGetPropertySafe(pBodies, joint_index, "effort_max"));
model->actuators.push_back(
RigidBodyActuator(name, model->bodies[joint_index].get(), reduction,
effort_min, effort_max));
}
// LOOP CONSTRAINTS
// DEBUG
// mexPrintf("constructModelmex: Calling compile\n");
// END_DEBUG
model->compile();
// mexPrintf("constructModelmex: Creating DrakeMexPointer\n");
plhs[0] = createDrakeMexPointer((void*)model, "RigidBodyTree",
DrakeMexPointerTypeId<RigidBodyTree>::value);
// DEBUG
// mexPrintf("constructModelmex: END\n");
// END_DEBUG
}