Exemple #1
0
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;
}
Exemple #4
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;
}
Exemple #6
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;
  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);
  }
}
Exemple #8
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);
}