void unigripper_motoman_plant_t::update_phys_configs(config_list_t& configs, unsigned& index) const
{
bool update = true;
VectorNd tmpVec = VectorNd::Zero(3);
VectorNd pos;
Matrix3d orient;
Quaternion quat;
double qw,qx,qy,qz;
for(unsigned i=0;i<simple_rigid_body_names.size();i++)
{
unsigned int id = body_ids[i];//model.GetBodyId(simple_rigid_body_names[i].c_str());
pos = CalcBodyToBaseCoordinates(model,Q,id,tmpVec,update);
update = false;
orient = CalcBodyWorldOrientation(model,Q,id,update);
quat = Quaternion::fromMatrix(orient);
augment_config_list(configs,index);
configs[index].first = config_names[i];
configs[index].second.set_position(pos[0],pos[1],pos[2]);
configs[index].second.set_orientation(quat.x(),quat.y(),quat.z(),quat.w());
if(std::isnan(quat.w()) || std::isnan(quat.x()) || std::isnan(quat.y()) || std::isnan(quat.z()) )
{
uni_convert_matrix_to_quaternion(orient,qx,qy,qz,qw);
configs[index].second.set_orientation(qx,qy,qz,qw);
}
index++;
}
}
void PoseTask::getSensedValue(const Eigen::VectorXd& q,
Eigen::VectorXd& xsensed)
{
if (taskType_ == 0)
xsensed = CalcBodyToBaseCoordinates(*model_, q, linkNum_,
localpos_, true);
if (taskType_ == 1)
{
std::cerr << "Not yet implemented ! TODO" << std::endl;
//TODO: Handle orientation (rot matrix)
//xsensed = CalcBodyWorldOrientation(*model_, q, linkNum_, true);
}
if (taskType_ == 2)
std::cerr << "Not yet implemented ! TODO" << std::endl;
}
void unigripper_motoman_plant_t::get_end_effector_configuration(config_t& effector_config)
{
double qw,qx,qy,qz;
bool update = true;
VectorNd tmpVec = VectorNd::Zero(3);
VectorNd pos;
Matrix3d orient;
Quaternion quat;
std::string ee = "vacuum_volume";
unsigned int id = model.GetBodyId(ee.c_str());
pos = CalcBodyToBaseCoordinates(model,Q,id,tmpVec,update);
update = false;
orient = CalcBodyWorldOrientation(model,Q,id,update);
quat = Quaternion::fromMatrix(orient);
effector_config.set_position(pos[0], pos[1], pos[2]);
effector_config.set_orientation(quat[0],quat[1],quat[2],quat[3]);
if(std::isnan(quat.w()) || std::isnan(quat.x()) || std::isnan(quat.y()) || std::isnan(quat.z()) )
{
uni_convert_matrix_to_quaternion(orient,qx,qy,qz,qw);
effector_config.set_orientation(qx,qy,qz,qw);
}
}
