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;
}
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);
}
