void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
if(nrhs!= 1)
{
mexErrMsgIdAndTxt("Drake:constraintCategorymex:BadInputs","Usage constraint_category = constraintCategorymex(constraint_ptr)");
}
RigidBodyConstraint* cnst = (RigidBodyConstraint*) getDrakeMexPointer(prhs[0]);
int category = cnst->getCategory();
switch(category)
{
case RigidBodyConstraint::SingleTimeKinematicConstraintCategory:
plhs[0] = mxCreateString("SingleTimeKinematicConstraint");
break;
case RigidBodyConstraint::MultipleTimeKinematicConstraintCategory:
plhs[0] = mxCreateString("MultipleTimeKinematicConstraint");
break;
case RigidBodyConstraint::QuasiStaticConstraintCategory:
plhs[0] = mxCreateString("QuasiStaticConstraint");
break;
case RigidBodyConstraint::PostureConstraintCategory:
plhs[0] = mxCreateString("PostureConstraint");
break;
case RigidBodyConstraint::MultipleTimeLinearPostureConstraintCategory:
plhs[0] = mxCreateString("MultipleTimeLinearPostureConstraint");
break;
case RigidBodyConstraint::SingleTimeLinearPostureConstraintCategory:
plhs[0] = mxCreateString("SingleTimeLinearPostureConstraint");
break;
default:
mexErrMsgIdAndTxt("Drake:constraintCategorymex:BadInputs","The constraint category is not supported");
break;
}
}
void mexFunction(int nlhs, mxArray *plhs[],int nrhs, const mxArray *prhs[])
{
RigidBodyConstraint* constraint = (RigidBodyConstraint*) getDrakeMexPointer(prhs[0]);
int constraint_type = constraint->getType();
mwSize strlen = mxGetNumberOfElements(prhs[1])+1;
char* field = new char[strlen];
mxGetString(prhs[1],field,strlen);
string field_str(field);
switch(constraint_type)
{
case RigidBodyConstraint::QuasiStaticConstraintType:
{
QuasiStaticConstraint* cnst = (QuasiStaticConstraint*) constraint;
if(field_str=="active")
{
// setActive(qsc_ptr,flag)
if(mxGetNumberOfElements(prhs[2]) != 1)
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","QuasiStaticConstraint:flag must be a single boolean");
}
bool* flag = mxGetLogicals(prhs[2]);
QuasiStaticConstraint* cnst_new = new QuasiStaticConstraint(*cnst);
cnst_new->setActive(*flag);
plhs[0] = createDrakeConstraintMexPointer((void*) cnst_new,"deleteRigidBodyConstraintmex","QuasiStaticConstraint");
}
else if(field_str=="factor")
{ // setShrinkFactor(qsc_ptr,factor)
if(mxGetNumberOfElements(prhs[2]) != 1)
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","QuasiStaticConstraint:shrink factor must be a double scalar");
}
double factor = mxGetScalar(prhs[2]);
if(factor<=0.0)
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","QuasiStaticConstraint:shrink factor should be a positive scalar");
}
QuasiStaticConstraint* cnst_new = new QuasiStaticConstraint(*cnst);
cnst_new->setShrinkFactor(factor);
plhs[0] = createDrakeConstraintMexPointer((void*) cnst_new,"deleteRigidBodyConstraintmex","QuasiStaticConstraint");
}
else if(field_str=="contact")
{
// addContact(qsc_ptr,body1, body1_pts, body2, body2_pts,...)
int num_new_bodies = (nrhs-2)/2;
int* new_bodies = new int[num_new_bodies];
MatrixXd* new_body_pts = new MatrixXd[num_new_bodies];
for(int idx = 0; idx<num_new_bodies; idx++)
{
new_bodies[idx] = (int) mxGetScalar(prhs[2+idx*2])-1;
int npts = mxGetN(prhs[3+idx*2]);
MatrixXd new_body_pts_tmp(3,npts);
memcpy(new_body_pts_tmp.data(),mxGetPr(prhs[3+idx*2]),sizeof(double)*3*npts);
new_body_pts[idx].resize(4,npts);
new_body_pts[idx].block(0,0,3,npts) = new_body_pts_tmp;
new_body_pts[idx].row(3) = MatrixXd::Ones(1,npts);
}
QuasiStaticConstraint* cnst_new = new QuasiStaticConstraint(*cnst);
cnst_new->addContact(num_new_bodies,new_bodies,new_body_pts);
plhs[0] = createDrakeConstraintMexPointer((void*) cnst_new,"deleteRigidBodyConstraintmex","QuasiStaticConstraint");
delete[] new_bodies;
delete[] new_body_pts;
}
else if(field_str=="robot")
{
RigidBodyManipulator* robot = (RigidBodyManipulator*) getDrakeMexPointer(prhs[2]);
QuasiStaticConstraint* cnst_new = new QuasiStaticConstraint(*cnst);
cnst_new->updateRobot(robot);
plhs[0] = createDrakeConstraintMexPointer((void*) cnst_new,"deleteRigidBodyConstraintmex","QuasiStaticConstraint");
}
else if(field_str=="robotnum")
{
int num_robot = mxGetNumberOfElements(prhs[2]);
double* robotnum_tmp = new double[num_robot];
int* robotnum = new int[num_robot];
memcpy(robotnum_tmp,mxGetPr(prhs[2]),sizeof(double)*num_robot);
for(int i = 0; i<num_robot; i++)
{
robotnum[i] = (int) robotnum_tmp[i]-1;
}
set<int> robotnumset(robotnum,robotnum+num_robot);
QuasiStaticConstraint* cnst_new = new QuasiStaticConstraint(*cnst);
cnst_new->updateRobotnum(robotnumset);
plhs[0] = createDrakeConstraintMexPointer((void*) cnst_new,"deleteRigidBodyConstraintmex","QuasiStaticConstraint");
delete[] robotnum_tmp;
delete[] robotnum;
}
else
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","QuasiStaticConstraint:argument 2 is not accepted");
}
}
break;
case RigidBodyConstraint::PostureConstraintType:
{
PostureConstraint* pc = (PostureConstraint*) constraint;
if(field_str=="bounds")
{ // setJointLimits(pc,joint_idx,lb,ub)
int num_idx = mxGetM(prhs[2]);
if(!mxIsNumeric(prhs[2]) || mxGetN(prhs[2]) != 1 || !mxIsNumeric(prhs[3]) || mxGetM(prhs[3]) != num_idx || mxGetN(prhs[3]) != 1 || !mxIsNumeric(prhs[4]) || mxGetM(prhs[4]) != num_idx || mxGetN(prhs[4]) != 1)
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","PostureConstraint:joint_idx, lb and ub must be of the same length numerical vector");
}
int* joint_idx = new int[num_idx];
for(int i = 0; i<num_idx; i++)
{
joint_idx[i] = (int) *(mxGetPr(prhs[2])+i)-1;
}
VectorXd lb(num_idx),ub(num_idx);
memcpy(lb.data(),mxGetPr(prhs[3]),sizeof(double)*num_idx);
memcpy(ub.data(),mxGetPr(prhs[4]),sizeof(double)*num_idx);
PostureConstraint* pc_new = new PostureConstraint(*pc);
pc_new->setJointLimits(num_idx,joint_idx,lb,ub);
delete[] joint_idx;
plhs[0] = createDrakeConstraintMexPointer((void*)pc_new,"deleteRigidBodyConstraintmex","PostureConstraint");
}
else
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","PostureConstraint: argument 2 is not accepted");
}
}
break;
case RigidBodyConstraint::AllBodiesClosestDistanceConstraintType:
{
AllBodiesClosestDistanceConstraint* cnst = (AllBodiesClosestDistanceConstraint*) constraint;
if(field_str=="robot")
{
RigidBodyManipulator* robot = (RigidBodyManipulator*) getDrakeMexPointer(prhs[2]);
AllBodiesClosestDistanceConstraint* cnst_new = new AllBodiesClosestDistanceConstraint(*cnst);
cnst_new->updateRobot(robot);
plhs[0] = createDrakeConstraintMexPointer((void*) cnst_new,"deleteRigidBodyConstraintmex","AllBodiesClosestDistanceConstraint");
}
else
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","AllBodiesClosestDistanceConstraint:argument 2 is not accepted");
}
}
break;
case RigidBodyConstraint::WorldEulerConstraintType:
{
WorldEulerConstraint* cnst = (WorldEulerConstraint*) constraint;
if(field_str=="robot")
{
RigidBodyManipulator* robot = (RigidBodyManipulator*) getDrakeMexPointer(prhs[2]);
WorldEulerConstraint* cnst_new = new WorldEulerConstraint(*cnst);
cnst_new->updateRobot(robot);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst_new,"deleteRigidBodyConstraintmex","WorldEulerConstraint");
}
else
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","WorldEulerConstraint:argument 2 is not accepted");
}
}
break;
case RigidBodyConstraint::WorldGazeDirConstraintType:
{
WorldGazeDirConstraint* cnst = (WorldGazeDirConstraint*) constraint;
if(field_str=="robot")
{
RigidBodyManipulator* robot = (RigidBodyManipulator*) getDrakeMexPointer(prhs[2]);
WorldGazeDirConstraint* cnst_new = new WorldGazeDirConstraint(*cnst);
cnst_new->updateRobot(robot);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst_new,"deleteRigidBodyConstraintmex","WorldGazeDirConstraint");
}
else
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","WorldGazeDirConstraint:argument 2 is not accepted");
}
}
break;
case RigidBodyConstraint::WorldGazeOrientConstraintType:
{
WorldGazeOrientConstraint* cnst = (WorldGazeOrientConstraint*) constraint;
if(field_str=="robot")
{
RigidBodyManipulator* robot = (RigidBodyManipulator*) getDrakeMexPointer(prhs[2]);
WorldGazeOrientConstraint* cnst_new = new WorldGazeOrientConstraint(*cnst);
cnst_new->updateRobot(robot);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst_new,"deleteRigidBodyConstraintmex","WorldGazeOrientConstraint");
}
else
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","WorldGazeOrientConstraint:argument 2 is not accepted");
}
}
break;
case RigidBodyConstraint::WorldGazeTargetConstraintType:
{
WorldGazeTargetConstraint* cnst = (WorldGazeTargetConstraint*) constraint;
if(field_str=="robot")
{
RigidBodyManipulator* robot = (RigidBodyManipulator*) getDrakeMexPointer(prhs[2]);
WorldGazeTargetConstraint* cnst_new = new WorldGazeTargetConstraint(*cnst);
cnst_new->updateRobot(robot);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst_new,"deleteRigidBodyConstraintmex","WorldGazeTargetConstraint");
}
else
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","WorldGazeTargetConstraint:argument 2 is not accepted");
}
}
break;
case RigidBodyConstraint::RelativeGazeTargetConstraintType:
{
RelativeGazeTargetConstraint* cnst = (RelativeGazeTargetConstraint*) constraint;
if(field_str=="robot")
{
RigidBodyManipulator* robot = (RigidBodyManipulator*) getDrakeMexPointer(prhs[2]);
RelativeGazeTargetConstraint* cnst_new = new RelativeGazeTargetConstraint(*cnst);
cnst_new->updateRobot(robot);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst_new,"deleteRigidBodyConstraintmex","RelativeGazeTargetConstraint");
}
else
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","RelativeGazeTargetConstraint:argument 2 is not accepted");
}
}
break;
case RigidBodyConstraint::RelativeGazeDirConstraintType:
{
RelativeGazeDirConstraint* cnst = (RelativeGazeDirConstraint*) constraint;
if(field_str=="robot")
{
RigidBodyManipulator* robot = (RigidBodyManipulator*) getDrakeMexPointer(prhs[2]);
RelativeGazeDirConstraint* cnst_new = new RelativeGazeDirConstraint(*cnst);
cnst_new->updateRobot(robot);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst_new,"deleteRigidBodyConstraintmex","RelativeGazeDirConstraint");
}
else
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","RelativeGazeDirConstraint:argument 2 is not accepted");
}
}
break;
case RigidBodyConstraint::WorldCoMConstraintType:
{
WorldCoMConstraint* cnst = (WorldCoMConstraint*) constraint;
if(field_str=="robot")
{
RigidBodyManipulator* robot = (RigidBodyManipulator*) getDrakeMexPointer(prhs[2]);
WorldCoMConstraint* cnst_new = new WorldCoMConstraint(*cnst);
cnst_new->updateRobot(robot);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst_new,"deleteRigidBodyConstraintmex","WorldCoMConstraint");
}
else if(field_str=="robotnum")
{
int num_robot = mxGetNumberOfElements(prhs[2]);
double* robotnum_tmp = new double[num_robot];
int* robotnum = new int[num_robot];
memcpy(robotnum_tmp,mxGetPr(prhs[2]),sizeof(double)*num_robot);
for(int i = 0; i<num_robot; i++)
{
robotnum[i] = (int) robotnum_tmp[i]-1;
}
set<int> robotnumset(robotnum,robotnum+num_robot);
WorldCoMConstraint* cnst_new = new WorldCoMConstraint(*cnst);
cnst_new->updateRobotnum(robotnumset);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst_new,"deleteRigidBodyConstraintmex","WorldCoMConstraint");
delete[] robotnum_tmp;
delete[] robotnum;
}
else
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","WorldCoMConstraint:argument 2 is not accepted");
}
}
break;
case RigidBodyConstraint::WorldPositionConstraintType:
{
WorldPositionConstraint* cnst = (WorldPositionConstraint*) constraint;
if(field_str=="robot")
{
RigidBodyManipulator* robot = (RigidBodyManipulator*) getDrakeMexPointer(prhs[2]);
WorldPositionConstraint* cnst_new = new WorldPositionConstraint(*cnst);
cnst_new->updateRobot(robot);
plhs[0] = createDrakeConstraintMexPointer((void*) cnst_new,"deleteRigidBodyConstraintmex","WorldPositionConstraint");
}
else
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","WorldPositionConstraint:argument 2 is not accepted");
}
}
break;
case RigidBodyConstraint::WorldPositionInFrameConstraintType:
{
WorldPositionInFrameConstraint* cnst = (WorldPositionInFrameConstraint*) constraint;
if(field_str=="robot")
{
RigidBodyManipulator* robot = (RigidBodyManipulator*) getDrakeMexPointer(prhs[2]);
WorldPositionInFrameConstraint* cnst_new = new WorldPositionInFrameConstraint(*cnst);
cnst_new->updateRobot(robot);
plhs[0] = createDrakeConstraintMexPointer((void*) cnst_new,"deleteRigidBodyConstraintmex","WorldPositionInFrameConstraint");
}
else
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","WorldPositionInFrameConstraint:argument 2 is not accepted");
}
}
break;
case RigidBodyConstraint::WorldQuatConstraintType:
{
WorldQuatConstraint* cnst = (WorldQuatConstraint*) constraint;
if(field_str=="robot")
{
RigidBodyManipulator* robot = (RigidBodyManipulator*) getDrakeMexPointer(prhs[2]);
WorldQuatConstraint* cnst_new = new WorldQuatConstraint(*cnst);
cnst_new->updateRobot(robot);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst_new,"deleteRigidBodyConstraintmex","WorldQuatConstraint");
}
else
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","WorldQuatConstraint:argument 2 is not accepted");
}
}
break;
case RigidBodyConstraint::Point2PointDistanceConstraintType:
{
Point2PointDistanceConstraint* cnst = (Point2PointDistanceConstraint*) constraint;
if(field_str=="robot")
{
RigidBodyManipulator* robot = (RigidBodyManipulator*) getDrakeMexPointer(prhs[2]);
Point2PointDistanceConstraint* cnst_new = new Point2PointDistanceConstraint(*cnst);
cnst_new->updateRobot(robot);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst_new,"deleteRigidBodyConstraintmex","Point2PointDistanceConstraint");
}
else
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","Point2PointDistanceConstraint:argument 2 is not accepted");
}
}
break;
case RigidBodyConstraint::Point2LineSegDistConstraintType:
{
Point2LineSegDistConstraint* cnst = (Point2LineSegDistConstraint*) constraint;
if(field_str=="robot")
{
RigidBodyManipulator* robot = (RigidBodyManipulator*) getDrakeMexPointer(prhs[2]);
Point2LineSegDistConstraint* cnst_new = new Point2LineSegDistConstraint(*cnst);
cnst_new->updateRobot(robot);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst_new,"deleteRigidBodyConstraintmex","Point2LineSegDistConstraint");
}
else
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","Point2LineSegDistConstraint:argument 2 is not accepted");
}
}
break;
case RigidBodyConstraint::WorldFixedPositionConstraintType:
{
WorldFixedPositionConstraint* cnst = (WorldFixedPositionConstraint*) constraint;
if(field_str=="robot")
{
RigidBodyManipulator* robot = (RigidBodyManipulator*) getDrakeMexPointer(prhs[2]);
WorldFixedPositionConstraint* cnst_new = new WorldFixedPositionConstraint(*cnst);
cnst_new->updateRobot(robot);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst_new,"deleteRigidBodyConstraintmex","WorldFixedPositionConstraint");
}
else
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","WorldFixedPositionConstraint:argument 2 is not accepted");
}
}
break;
case RigidBodyConstraint::WorldFixedOrientConstraintType:
{
WorldFixedOrientConstraint* cnst = (WorldFixedOrientConstraint*) constraint;
if(field_str=="robot")
{
RigidBodyManipulator* robot = (RigidBodyManipulator*) getDrakeMexPointer(prhs[2]);
WorldFixedOrientConstraint* cnst_new = new WorldFixedOrientConstraint(*cnst);
cnst_new->updateRobot(robot);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst_new,"deleteRigidBodyConstraintmex","WorldFixedOrientConstraint");
}
else
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","WorldFixedOrientConstraint:argument 2 is not accepted");
}
}
break;
case RigidBodyConstraint::WorldFixedBodyPoseConstraintType:
{
WorldFixedBodyPoseConstraint* cnst = (WorldFixedBodyPoseConstraint*) constraint;
if(field_str=="robot")
{
RigidBodyManipulator* robot = (RigidBodyManipulator*) getDrakeMexPointer(prhs[2]);
WorldFixedBodyPoseConstraint* cnst_new = new WorldFixedBodyPoseConstraint(*cnst);
cnst_new->updateRobot(robot);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst_new,"deleteRigidBodyConstraintmex","WorldFixedBodyPoseConstraint");
}
else
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","WorldFixedBodyPoseConstraint:argument 2 is not accepted");
}
}
break;
case RigidBodyConstraint::RelativePositionConstraintType:
{
RelativePositionConstraint* cnst = static_cast<RelativePositionConstraint*>(constraint);
if(field_str == "robot")
{
RigidBodyManipulator* robot = (RigidBodyManipulator*) getDrakeMexPointer(prhs[2]);
RelativePositionConstraint* cnst_new = new RelativePositionConstraint(*cnst);
cnst_new->updateRobot(robot);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst_new,"deleteRigidBodyConstraintmex","RelativePositionConstraint");
}
else
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","RelativePositionConstraint:argument 2 is not accepted");
}
}
break;
case RigidBodyConstraint::RelativeQuatConstraintType:
{
RelativeQuatConstraint* cnst = static_cast<RelativeQuatConstraint*>(constraint);
if(field_str == "robot")
{
RigidBodyManipulator* robot = (RigidBodyManipulator*) getDrakeMexPointer(prhs[2]);
RelativeQuatConstraint* cnst_new = new RelativeQuatConstraint(*cnst);
cnst_new->updateRobot(robot);
plhs[0] = createDrakeConstraintMexPointer((void*)cnst_new,"deleteRigidBodyConstraintmex","RelativeQuatConstraint");
}
else
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","RelativeQuatConstraint:argument 2 is not accepted");
}
}
break;
case RigidBodyConstraint::MinDistanceConstraintType:
{
MinDistanceConstraint* cnst = (MinDistanceConstraint*) constraint;
if(field_str=="robot")
{
RigidBodyManipulator* robot = (RigidBodyManipulator*) getDrakeMexPointer(prhs[2]);
MinDistanceConstraint* cnst_new = new MinDistanceConstraint(*cnst);
cnst_new->updateRobot(robot);
plhs[0] = createDrakeConstraintMexPointer((void*) cnst_new,"deleteRigidBodyConstraintmex","MinDistanceConstraint");
}
else
{
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","MinDistanceConstraint:argument 2 is not accepted");
}
}
break;
default:
mexErrMsgIdAndTxt("Drake:updatePtrRigidBodyConstraintmex:BadInputs","Unsupported constraint type");
break;
}
delete[] field;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
if(nrhs!= 1)
{
mexErrMsgIdAndTxt("Drake:constraintTypemex:BadInputs","Usage constraint_type = constraintTypemex(constraint_ptr)");
}
RigidBodyConstraint* cnst = (RigidBodyConstraint*) getDrakeMexPointer(prhs[0]);
int type = cnst->getType();
switch(type)
{
case RigidBodyConstraint::QuasiStaticConstraintType:
plhs[0] = mxCreateString("QuasiStaticConstraint");
break;
case RigidBodyConstraint::PostureConstraintType:
plhs[0] = mxCreateString("PostureConstraint");
break;
case RigidBodyConstraint::SingleTimeLinearPostureConstraintType:
plhs[0] = mxCreateString("SingleTimeLinearPostureConstraint");
break;
case RigidBodyConstraint::AllBodiesClosestDistanceConstraintType:
plhs[0] = mxCreateString("AllBodiesClosestDistanceConstraint");
break;
case RigidBodyConstraint::WorldEulerConstraintType:
plhs[0] = mxCreateString("WorldEulerConstraint");
break;
case RigidBodyConstraint::WorldGazeDirConstraintType:
plhs[0] = mxCreateString("WorldGazeDirConstraint");
break;
case RigidBodyConstraint::WorldGazeOrientConstraintType:
plhs[0] = mxCreateString("WorldGazeOrientConstraint");
break;
case RigidBodyConstraint::WorldGazeTargetConstraintType:
plhs[0] = mxCreateString("WorldGazeTargetConstraint");
break;
case RigidBodyConstraint::RelativeGazeTargetConstraintType:
plhs[0] = mxCreateString("RelativeGazeTargetConstraint");
break;
case RigidBodyConstraint::RelativeGazeDirConstraintType:
plhs[0] = mxCreateString("RelativeGazeDirConstraint");
break;
case RigidBodyConstraint::WorldCoMConstraintType:
plhs[0] = mxCreateString("WorldCoMConstraint");
break;
case RigidBodyConstraint::WorldPositionConstraintType:
plhs[0] = mxCreateString("WorldPositionConstraint");
break;
case RigidBodyConstraint::WorldPositionInFrameConstraintType:
plhs[0] = mxCreateString("WorldPositionInFrameConstraint");
break;
case RigidBodyConstraint::WorldQuatConstraintType:
plhs[0] = mxCreateString("WorldQuatConstraint");
break;
case RigidBodyConstraint::Point2PointDistanceConstraintType:
plhs[0] = mxCreateString("Point2PointDistanceConstraint");
break;
case RigidBodyConstraint::Point2LineSegDistConstraintType:
plhs[0] = mxCreateString("Point2LineSegDistConstraint");
break;
case RigidBodyConstraint::WorldFixedPositionConstraintType:
plhs[0] = mxCreateString("WorldFixedPositionConstraint");
break;
case RigidBodyConstraint::WorldFixedOrientConstraintType:
plhs[0] = mxCreateString("WorldFixedOrientConstraint");
break;
case RigidBodyConstraint::WorldFixedBodyPoseConstraintType:
plhs[0] = mxCreateString("WorldFixedBodyPoseConstraint");
break;
case RigidBodyConstraint::PostureChangeConstraintType:
plhs[0] = mxCreateString("PostureChangeConstraint");
break;
case RigidBodyConstraint::RelativePositionConstraintType:
plhs[0] = mxCreateString("RelativePositionConstraint");
break;
case RigidBodyConstraint::RelativeQuatConstraintType:
plhs[0] = mxCreateString("RelativeQuatConstraint");
break;
case RigidBodyConstraint::MinDistanceConstraintType:
plhs[0] = mxCreateString("MinDistanceConstraint");
break;
case RigidBodyConstraint::GravityCompensationTorqueConstraintType:
plhs[0] = mxCreateString("GravityCompensationTorqueConstraint");
break;
default:
mexErrMsgIdAndTxt("Drake:constraintTypemex:BadInputs","The constraint type is not supported");
break;
}
}
void inverseKinBackend(
RigidBodyTree* model, const int nT,
const double* t, const MatrixBase<DerivedA>& q_seed,
const MatrixBase<DerivedB>& q_nom, const int num_constraints,
RigidBodyConstraint** const constraint_array,
const IKoptions& ikoptions, MatrixBase<DerivedC>* q_sol,
int* info, std::vector<std::string>* infeasible_constraint) {
// Validate some basic parameters of the input.
if (q_seed.rows() != model->number_of_positions() || q_seed.cols() != nT ||
q_nom.rows() != model->number_of_positions() || q_nom.cols() != nT) {
throw std::runtime_error(
"Drake::inverseKinBackend: q_seed and q_nom must be of size "
"nq x nT");
}
KinematicsCacheHelper<double> kin_helper(model->bodies);
// TODO(sam.creasey) I really don't like rebuilding the
// OptimizationProblem for every timestep, but it's not possible to
// enable/disable (or even remove) a constraint from an
// OptimizationProblem between calls to Solve() currently, so
// there's not actually another way.
for (int t_index = 0; t_index < nT; t_index++) {
OptimizationProblem prog;
SetIKSolverOptions(ikoptions, &prog);
DecisionVariableView vars =
prog.AddContinuousVariables(model->number_of_positions());
MatrixXd Q;
ikoptions.getQ(Q);
auto objective = std::make_shared<InverseKinObjective>(model, Q);
prog.AddCost(objective, {vars});
for (int i = 0; i < num_constraints; i++) {
RigidBodyConstraint* constraint = constraint_array[i];
const int constraint_category = constraint->getCategory();
if (constraint_category ==
RigidBodyConstraint::SingleTimeKinematicConstraintCategory) {
SingleTimeKinematicConstraint* stc =
static_cast<SingleTimeKinematicConstraint*>(constraint);
if (!stc->isTimeValid(&t[t_index])) { continue; }
auto wrapper = std::make_shared<SingleTimeKinematicConstraintWrapper>(
stc, &kin_helper);
prog.AddConstraint(wrapper, {vars});
} else if (constraint_category ==
RigidBodyConstraint::PostureConstraintCategory) {
PostureConstraint* pc = static_cast<PostureConstraint*>(constraint);
if (!pc->isTimeValid(&t[t_index])) { continue; }
VectorXd lb;
VectorXd ub;
pc->bounds(&t[t_index], lb, ub);
prog.AddBoundingBoxConstraint(lb, ub, {vars});
} else if (
constraint_category ==
RigidBodyConstraint::SingleTimeLinearPostureConstraintCategory) {
AddSingleTimeLinearPostureConstraint(
&t[t_index], constraint, model->number_of_positions(),
vars, &prog);
} else if (constraint_category ==
RigidBodyConstraint::QuasiStaticConstraintCategory) {
AddQuasiStaticConstraint(&t[t_index], constraint, &kin_helper,
vars, &prog);
} else if (constraint_category ==
RigidBodyConstraint::MultipleTimeKinematicConstraintCategory) {
throw std::runtime_error(
"MultipleTimeKinematicConstraint is not supported"
" in pointwise mode.");
} else if (
constraint_category ==
RigidBodyConstraint::MultipleTimeLinearPostureConstraintCategory) {
throw std::runtime_error(
"MultipleTimeLinearPostureConstraint is not supported"
" in pointwise mode.");
}
}
// Add a bounding box constraint from the model.
prog.AddBoundingBoxConstraint(
model->joint_limit_min, model->joint_limit_max,
{vars});
// TODO(sam.creasey) would this be faster if we stored the view
// instead of copying into a VectorXd?
objective->set_q_nom(q_nom.col(t_index));
if (!ikoptions.getSequentialSeedFlag() || (t_index == 0)) {
prog.SetInitialGuess(vars, q_seed.col(t_index));
} else {
prog.SetInitialGuess(vars, q_sol->col(t_index - 1));
}
SolutionResult result = prog.Solve();
prog.PrintSolution();
q_sol->col(t_index) = vars.value();
info[t_index] = GetIKSolverInfo(prog, result);
}
}
