Esempio n. 1
0
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);
}
Esempio n. 2
0
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);
    }
}
Esempio n. 3
0
/**
 * @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);
}
Esempio n. 5
0
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();
	*/
}
Esempio n. 6
0
    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
    }
Esempio n. 7
0
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;
};
Esempio n. 9
0
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();
}
Esempio n. 10
0
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;
}
Esempio n. 11
0
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;
    }
}
Esempio n. 12
0
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);
  }
}
Esempio n. 13
0
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();
}
Esempio n. 14
0
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;	
	}
}
}
Esempio n. 16
0
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;
}
Esempio n. 17
0
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();
}
Esempio n. 18
0
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();
}
Esempio n. 19
0
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());
}
Esempio n. 20
0
double index2k(const indices3d& ilist) {
    Vector3d tmp;
    transform(ilist.begin(), ilist.end(), tmp.data(), ki);
    return tmp.norm();
}
Esempio n. 21
0
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();
    }
}
Esempio n. 25
0
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
}