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); } }
void movable_body_plant_t::get_configuration( config_t& body_config ) const { body_config.set_position(_x, _y, _z); body_config.set_xyzw_orientation(_qx, _qy, _qz, _qw); }