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