void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
if (nrhs != 5) {
mexErrMsgIdAndTxt("Drake:doKinematicsmex:NotEnoughInputs", "Usage doKinematicsmex(model_ptr,q,compute_gradients,v,compute_JdotV)");
}
// first get the model_ptr back from matlab
RigidBodyManipulator *model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[0]);
Map<VectorXd> q(mxGetPrSafe(prhs[1]), mxGetNumberOfElements(prhs[1]));
if (q.rows() != model->num_positions)
mexErrMsgIdAndTxt("Drake:doKinematicsmex:BadInputs", "q must be size %d x 1", model->num_positions);
bool compute_gradients = (bool) (mxGetLogicals(prhs[2]))[0];
bool compute_Jdotv = (bool) (mxGetLogicals(prhs[4]))[0];
if (mxGetNumberOfElements(prhs[3]) > 0) {
auto v = Map<VectorXd>(mxGetPrSafe(prhs[3]), mxGetNumberOfElements(prhs[3]));
if (v.rows() != model->num_velocities)
mexErrMsgIdAndTxt("Drake:doKinematicsmex:BadInputs", "v must be size %d x 1", model->num_velocities);
model->doKinematics(q, v, compute_gradients, compute_Jdotv);
}
else {
Map<VectorXd> v(nullptr, 0, 1);
model->doKinematics(q, v, compute_gradients, compute_Jdotv);
}
}
void testUserGradients(const RigidBodyManipulator &model, int ntests) {
VectorXd q(model.num_positions);
KinematicsCache<double> cache(model.bodies, 1);
for (int i = 0; i < ntests; i++) {
model.getRandomConfiguration(q, generator);
cache.initialize(q);
model.doKinematics(cache, false);
auto H_gradientvar = model.massMatrix(cache, 1);
}
}
int main(int argc, char* argv[])
{
if (argc<2) {
cerr << "Usage: urdfCollisionTest urdf_filename" << endl;
exit(-1);
}
RigidBodyManipulator* model = new RigidBodyManipulator(argv[1]);
if (!model) {
cerr << "ERROR: Failed to load model from " << argv[1] << endl;
return -1;
}
// run kinematics with second derivatives 100 times
VectorXd q = VectorXd::Zero(model->num_positions);
int i;
if (argc>=2+model->num_positions) {
for (i=0; i<model->num_positions; i++)
sscanf(argv[2+i],"%lf",&q(i));
}
// for (i=0; i<model->num_dof; i++)
// q(i)=(double)rand() / RAND_MAX;
KinematicsCache<double> cache = model->doKinematics(q, 0);
// }
VectorXd phi;
Matrix3Xd normal, xA, xB;
vector<int> bodyA_idx, bodyB_idx;
model->collisionDetect(cache, phi,normal,xA,xB,bodyA_idx,bodyB_idx);
cout << "=======" << endl;
for (int j=0; j<phi.rows(); ++j) {
cout << phi(j) << " ";
for (int i=0; i<3; ++i) {
cout << normal(i,j) << " ";
}
for (int i=0; i<3; ++i) {
cout << xA(i,j) << " ";
}
for (int i=0; i<3; ++i) {
cout << xB(i,j) << " ";
}
cout << model->bodies[bodyA_idx.at(j)]->linkname << " ";
cout << model->bodies[bodyB_idx.at(j)]->linkname << endl;
}
delete model;
return 0;
}
int main(int argc, char* argv[])
{
if (argc<2) {
cerr << "Usage: urdfKinTest urdf_filename" << endl;
exit(-1);
}
RigidBodyManipulator* model = new RigidBodyManipulator(argv[1]);
if (!model) {
cerr << "ERROR: Failed to load model from " << argv[1] << endl;
return -1;
}
cout << "=======" << endl;
// run kinematics with second derivatives 100 times
Eigen::VectorXd q = Eigen::VectorXd::Zero(model->num_positions);
Eigen::VectorXd v = Eigen::VectorXd::Zero(model->num_velocities);
int i;
if (argc>=2+model->num_positions) {
for (i=0; i<model->num_positions; i++)
sscanf(argv[2+i],"%lf",&q(i));
}
// for (i=0; i<model->num_dof; i++)
// q(i)=(double)rand() / RAND_MAX;
KinematicsCache<double> cache = model->doKinematics(q, v, 0);
// }
// const Vector4d zero(0,0,0,1);
Eigen::Vector3d zero = Eigen::Vector3d::Zero();
Eigen::Matrix<double,6,1> pt;
for (i=0; i<model->bodies.size(); i++) {
// model->forwardKin(i,zero,1,pt);
auto pt = model->forwardKin(cache, zero, i, 0, 1, 0);
// cout << i << ": forward kin: " << model->bodies[i].linkname << " is at " << pt.transpose() << endl;
cout << model->bodies[i]->linkname << " ";
cout << pt.value().transpose() << endl;
// for (int j=0; j<pt.size(); j++)
// cout << pt(j) << " ";
}
auto phi = model->positionConstraints<double>(cache,1);
cout << "phi = " << phi.value().transpose() << endl;
delete model;
return 0;
}
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
if(nrhs!= 3 && nrhs != 4)
{
mexErrMsgIdAndTxt("Drake:testQuasiStaticConstraintmex:BadInputs","Usage [active,num_weights,c,dc] = testQuasiStaticConstraintmex(qsc_ptr,q,weights,t)");
}
double t;
double* t_ptr;
if(nrhs == 4&&mxGetNumberOfElements(prhs[3])== 1)
{
t = mxGetScalar(prhs[3]);
t_ptr = &t;
}
else
{
t_ptr = nullptr;
}
QuasiStaticConstraint* qsc = (QuasiStaticConstraint*) getDrakeMexPointer(prhs[0]);
bool active = qsc->isActive();
RigidBodyManipulator* model = qsc->getRobotPointer();
int nq = model->num_dof;
double *q = new double[nq];
memcpy(q,mxGetPr(prhs[1]),sizeof(double)*nq);
int num_weights = qsc->getNumWeights();
double* weights = new double[num_weights];
memcpy(weights,mxGetPr(prhs[2]),sizeof(double)*num_weights);
int num_qsc_cnst = qsc->getNumConstraint(t_ptr);
VectorXd c(num_qsc_cnst-1);
MatrixXd dc(num_qsc_cnst-1,nq+num_weights);
model->doKinematics(q);
qsc->eval(t_ptr,weights,c,dc);
VectorXd lb,ub;
lb.resize(num_qsc_cnst-1);
ub.resize(num_qsc_cnst-1);
qsc->bounds(t_ptr,lb,ub);
plhs[0] = mxCreateLogicalScalar(active);
plhs[1] = mxCreateDoubleScalar((double) num_weights);
plhs[2] = mxCreateDoubleMatrix(num_qsc_cnst-1,1,mxREAL);
memcpy(mxGetPr(plhs[2]),c.data(),sizeof(double)*(num_qsc_cnst-1));
plhs[3] = mxCreateDoubleMatrix(num_qsc_cnst-1,nq+num_weights,mxREAL);
memcpy(mxGetPr(plhs[3]),dc.data(),sizeof(double)*dc.size());
plhs[4] = mxCreateDoubleMatrix(num_qsc_cnst-1,1,mxREAL);
memcpy(mxGetPr(plhs[4]),lb.data(),sizeof(double)*(num_qsc_cnst-1));
plhs[5] = mxCreateDoubleMatrix(num_qsc_cnst-1,1,mxREAL);
memcpy(mxGetPr(plhs[5]),ub.data(),sizeof(double)*(num_qsc_cnst-1));
delete[] q;
delete[] weights;
}
int main()
{
RigidBodyManipulator rbm("examples/Atlas/urdf/atlas_minimal_contact.urdf");
RigidBodyManipulator* model = &rbm;
if(!model)
{
cerr<<"ERROR: Failed to load model"<<endl;
}
Vector2d tspan;
tspan<<0,1;
VectorXd q0 = VectorXd::Zero(model->num_positions);
// The state frame of cpp model does not match with the state frame of MATLAB model, since the dofname_to_dofnum is different in cpp and MATLAB
q0(3) = 0.8;
Vector3d com_lb = Vector3d::Zero();
Vector3d com_ub = Vector3d::Zero();
com_lb(2) = 0.9;
com_ub(2) = 1.0;
WorldCoMConstraint* com_kc = new WorldCoMConstraint(model,com_lb,com_ub,tspan);
int num_constraints = 1;
RigidBodyConstraint** constraint_array = new RigidBodyConstraint*[num_constraints];
constraint_array[0] = com_kc;
IKoptions ikoptions(model);
VectorXd q_sol(model->num_positions);
int info;
vector<string> infeasible_constraint;
inverseKin(model,q0,q0,num_constraints,constraint_array,q_sol,info,infeasible_constraint,ikoptions);
printf("INFO = %d\n",info);
if(info != 1)
{
return 1;
}
VectorXd v = VectorXd::Zero(0);
model->doKinematics(q_sol, v);
Vector3d com = model->centerOfMass<double>(0).value();
printf("%5.2f\n%5.2f\n%5.2f\n",com(0),com(1),com(2));
/*MATFile *presultmat;
presultmat = matOpen("q_sol.mat","w");
mxArray* pqsol = mxCreateDoubleMatrix(model->num_dof,1,mxREAL);
memcpy(mxGetPrSafe(pqsol),q_sol.data(),sizeof(double)*model->num_dof);
matPutVariable(presultmat,"q_sol",pqsol);
matClose(presultmat);*/
delete com_kc;
delete[] constraint_array;
return 0;
}
void testAutoDiffScalarGradients(const RigidBodyManipulator &model, int ntests) {
const int NUM_POSITIONS = Dynamic;
typedef Matrix<double, NUM_POSITIONS, 1> DerivativeType;
typedef AutoDiffScalar<DerivativeType> TaylorVar;
VectorXd q(model.num_positions);
Matrix<TaylorVar, NUM_POSITIONS, 1> q_taylorvar;
auto grad = Matrix<double, NUM_POSITIONS, NUM_POSITIONS>::Identity(model.num_positions, model.num_positions).eval();
KinematicsCache<TaylorVar> cache(model.bodies, 0);
for (int i = 0; i < ntests; i++) {
model.getRandomConfiguration(q, generator);
q_taylorvar = q.cast<TaylorVar>().eval();
gradientMatrixToAutoDiff(grad, q_taylorvar);
cache.initialize(q_taylorvar);
model.doKinematics(cache, false);
auto H_taylorvar = model.massMatrix(cache, 0);
}
}
int main()
{
RigidBodyManipulator rbm("examples/Atlas/urdf/atlas_minimal_contact.urdf");
RigidBodyManipulator* model = &rbm;
if(!model)
{
cerr<<"ERROR: Failed to load model"<<endl;
}
Vector2d tspan;
tspan<<0,1;
int l_hand;
int r_hand;
//int l_foot;
//int r_foot;
for(int i = 0;i<model->bodies.size();i++)
{
if(model->bodies[i]->linkname.compare(string("l_hand")))
{
l_hand = i;
}
else if(model->bodies[i]->linkname.compare(string("r_hand")))
{
r_hand = i;
}
//else if(model->bodies[i].linkname.compare(string("l_foot")))
//{
// l_foot = i;
//}
//else if(model->bodies[i].linkname.compare(string("r_foot")))
//{
// r_foot = i;
//}
}
int nq = model->num_positions;
VectorXd qstar = VectorXd::Zero(nq);
qstar(3) = 0.8;
KinematicsCache<double> cache = model->doKinematics(qstar, 0);
Vector3d com0 = model->centerOfMass(cache, 0).value();
Vector3d r_hand_pt = Vector3d::Zero();
Vector3d rhand_pos0 = model->forwardKin(cache, r_hand_pt, r_hand, 0, 0, 0).value();
int nT = 4;
double* t = new double[nT];
double dt = 1.0/(nT-1);
for(int i = 0;i<nT;i++)
{
t[i] = dt*i;
}
MatrixXd q0 = qstar.replicate(1,nT);
VectorXd qdot0 = VectorXd::Zero(model->num_velocities);
Vector3d com_lb = com0;
com_lb(0) = std::numeric_limits<double>::quiet_NaN();
com_lb(1) = std::numeric_limits<double>::quiet_NaN();
Vector3d com_ub = com0;
com_ub(0) = std::numeric_limits<double>::quiet_NaN();
com_ub(1) = std::numeric_limits<double>::quiet_NaN();
com_ub(2) = com0(2)+0.5;
WorldCoMConstraint* com_kc = new WorldCoMConstraint(model,com_lb,com_ub);
Vector3d rhand_pos_lb = rhand_pos0;
rhand_pos_lb(0) +=0.1;
rhand_pos_lb(1) +=0.05;
rhand_pos_lb(2) +=0.25;
Vector3d rhand_pos_ub = rhand_pos_lb;
rhand_pos_ub(2) += 0.25;
Vector2d tspan_end;
tspan_end<<t[nT-1],t[nT-1];
WorldPositionConstraint* kc_rhand = new WorldPositionConstraint(model,r_hand,r_hand_pt,rhand_pos_lb,rhand_pos_ub,tspan_end);
int num_constraints = 2;
RigidBodyConstraint** constraint_array = new RigidBodyConstraint*[num_constraints];
constraint_array[0] = com_kc;
constraint_array[1] = kc_rhand;
IKoptions ikoptions(model);
MatrixXd q_sol(model->num_positions,nT);
MatrixXd qdot_sol(model->num_velocities,nT);
MatrixXd qddot_sol(model->num_positions,nT);
int info = 0;
vector<string> infeasible_constraint;
inverseKinTraj(model,nT,t,qdot0,q0,q0,num_constraints,constraint_array,q_sol,qdot_sol,qddot_sol,info,infeasible_constraint,ikoptions);
printf("INFO = %d\n",info);
if(info != 1)
{
cerr<<"Failure"<<endl;
return 1;
}
ikoptions.setFixInitialState(false);
ikoptions.setMajorIterationsLimit(500);
inverseKinTraj(model,nT,t,qdot0,q0,q0,num_constraints,constraint_array,q_sol,qdot_sol,qddot_sol,info,infeasible_constraint,ikoptions);
printf("INFO = %d\n",info);
if(info != 1)
{
cerr<<"Failure"<<endl;
return 1;
}
RowVectorXd t_inbetween(5);
t_inbetween << 0.1,0.15,0.3,0.4,0.6;
ikoptions.setAdditionaltSamples(t_inbetween);
inverseKinTraj(model,nT,t,qdot0,q0,q0,num_constraints,constraint_array,q_sol,qdot_sol,qddot_sol,info,infeasible_constraint,ikoptions);
printf("INFO = %d\n",info);
if(info != 1)
{
cerr<<"Failure"<<endl;
return 1;
}
delete com_kc;
delete[] constraint_array;
delete[] t;
return 0;
}
void App::solveGazeProblem(){
// Publish the query for visualisation in Director
bot_core::pose_t goalMsg;
goalMsg.utime = rstate_.utime;
goalMsg.pos[0] = cl_cfg_.gazeGoal(0);
goalMsg.pos[1] = cl_cfg_.gazeGoal(1);
goalMsg.pos[2] = cl_cfg_.gazeGoal(2);
goalMsg.orientation[0] = 1;
goalMsg.orientation[1] = 0;
goalMsg.orientation[2] = 0;
goalMsg.orientation[3] = 0;
lcm_->publish("POSE_BODY_ALT", &goalMsg);
// Solve the IK problem for the neck:
VectorXd q_star(robotStateToDrakePosition(rstate_, dofMap_, model_.num_positions));
VectorXd q_sol(model_.num_positions);
int info = getConstraints(q_star, q_sol);
if (info != 1) {
std::cout << "Problem not solved\n";
return;
}
bool mode = 0;
if (mode==0){ // publish utorso-to-head as orientation, not properly tracking but works with different orientations
// Get the utorso to head frame:
int pelvis_link = model_.findLinkId("pelvis");
int head_link = model_.findLinkId("head");
int utorso_link = model_.findLinkId("torso");
KinematicsCache<double> cache = model_.doKinematics(q_sol,0);
Eigen::Isometry3d world_to_pelvis = matrixdToIsometry3d( model_.forwardKin(cache, Vector3d::Zero().eval(), pelvis_link, 0, 2, 0).value() );
Eigen::Isometry3d world_to_head = matrixdToIsometry3d( model_.forwardKin(cache, Vector3d::Zero().eval(), head_link, 0, 2, 0).value() );
Eigen::Isometry3d pelvis_to_head = world_to_head.inverse()*world_to_pelvis;
Eigen::Isometry3d pelvis_to_utorso = matrixdToIsometry3d( model_.forwardKin(cache, Vector3d::Zero().eval(), utorso_link, 0, 2, 0).value() ).inverse()*world_to_pelvis;
Eigen::Isometry3d utorso_to_head = pelvis_to_utorso.inverse()*pelvis_to_head;
// Apply 180 roll as head orientation control seems to be in z-up frame
Eigen::Isometry3d rotation_frame;
rotation_frame.setIdentity();
Eigen::Quaterniond quat = euler_to_quat( M_PI, 0, 0);
rotation_frame.rotate(quat);
utorso_to_head = utorso_to_head*rotation_frame;
bot_core::pose_t world_to_head_frame_pose_msg = getPoseAsBotPose(world_to_head, rstate_.utime);
lcm_->publish("POSE_VICON",&world_to_head_frame_pose_msg);// for debug
bot_core::pose_t utorso_to_head_frame_pose_msg = getPoseAsBotPose(utorso_to_head, rstate_.utime);
lcm_->publish("DESIRED_HEAD_ORIENTATION",&utorso_to_head_frame_pose_msg);// temp
}else{ // publish neck pitch and yaw joints as orientation. this works ok when robot is facing 1,0,0,0
// Fish out the two neck joints (in simulation) and send as a command:
std::vector<string> jointNames;
for (int i=0 ; i <model_.num_positions ; i++){
// std::cout << model.getPositionName(i) << " " << i << "\n";
jointNames.push_back( model_.getPositionName(i) ) ;
}
drc::robot_state_t robot_state_msg;
getRobotState(robot_state_msg, 0*1E6, q_sol , jointNames);
lcm_->publish("CANDIDATE_ROBOT_ENDPOSE",&robot_state_msg);
std::vector<std::string>::iterator it1 = find(jointNames.begin(),
jointNames.end(), "lowerNeckPitch");
int lowerNeckPitchIndex = std::distance(jointNames.begin(), it1);
float lowerNeckPitchAngle = q_sol[lowerNeckPitchIndex];
std::vector<std::string>::iterator it2 = find(jointNames.begin(),
jointNames.end(), "neckYaw");
int neckYawIndex = std::distance(jointNames.begin(), it2);
float neckYawAngle = q_sol[neckYawIndex];
std::cout << lowerNeckPitchAngle << " (" << lowerNeckPitchAngle*180.0/M_PI << ") is lowerNeckPitchAngle\n";
std::cout << neckYawAngle << " (" << neckYawAngle*180.0/M_PI << ") is neckYawAngle\n";
bot_core::pose_t headOrientationMsg;
headOrientationMsg.utime = rstate_.utime;
headOrientationMsg.pos[0] = 0;
headOrientationMsg.pos[1] = 0;
headOrientationMsg.pos[2] = 0;
Eigen::Quaterniond quat = euler_to_quat(0, lowerNeckPitchAngle, neckYawAngle);
headOrientationMsg.orientation[0] = quat.w();
headOrientationMsg.orientation[1] = quat.x();
headOrientationMsg.orientation[2] = quat.y();
headOrientationMsg.orientation[3] = quat.z();
lcm_->publish("DESIRED_HEAD_ORIENTATION",&headOrientationMsg);
lcm_->publish("POSE_VICON",&headOrientationMsg); // for debug
}
//std::cout << "Desired orientation sent, exiting\n";
//exit(-1);
}
