PoseGrabber::PoseGrabber(const ConfigReader &reader, const std::string &output_dir) : BasePoseGrabber(output_dir) {
self_name_ = "pose-grabber";
checkSelfName(reader.get_element("name"));
try{
model_.LoadData(reader.get_element("model-file"));
}
catch (std::runtime_error){
//e.g. no model
}
ifs_.open(reader.get_element("pose-file"));
if (!ifs_.is_open()){
throw std::runtime_error("Error, could not open file: " + reader.get_element("pose-file"));
}
ifs_.exceptions(std::ifstream::eofbit);
save_dir_ = output_dir;
ofs_file_ = save_dir_ + "/" + reader.get_element("output-pose-file");
}
BaseDaVinciPoseGrabber::BaseDaVinciPoseGrabber(const ConfigReader &reader, const std::string &output_dir) : BasePoseGrabber(output_dir) {
try{
model_.LoadData(reader.get_element("model-file"));
}
catch (std::runtime_error){
//no model (e.g. tracking camera)
}
}
SE3DaVinciPoseGrabber::SE3DaVinciPoseGrabber(const ConfigReader &reader, const std::string &output_dir, bool check_type) : BaseDaVinciPoseGrabber(reader, output_dir) {
if (check_type){
self_name_ = "se3-davinci-grabber";
checkSelfName(reader.get_element("name"));
}
std::string rotation_type = reader.get_element("rotation-type");
if (rotation_type == "euler") rotation_type_ = LoadType::EULER;
else if (rotation_type == "quaternion") rotation_type_ = LoadType::QUATERNION;
else if (rotation_type == "matrix") rotation_type_ = LoadType::MATRIX;
else throw std::runtime_error("");
if (reader.get_element("joint") == "PSM1")
target_joint_ = davinci::DaVinciJoint::PSM1;
else if (reader.get_element("joint") == "PSM2")
target_joint_ = davinci::DaVinciJoint::PSM2;
else if (reader.get_element("joint") == "ECM")
target_joint_ = davinci::DaVinciJoint::ECM;
else
throw std::runtime_error("Error, bad joint");
ifs_.open(reader.get_element("pose-file"));
if (!ifs_.is_open()) throw std::runtime_error("Could not open file!\n");
ofs_file_ = output_dir + "/" + reader.get_element("output-pose-file");
num_wrist_joints_ = 4; //should this load from config file?
wrist_dh_params_ = std::vector<double>(num_wrist_joints_, 0.0);
wrist_offsets_ = std::vector<float>(num_wrist_joints_, 0.0);
try{
SetupOffsets(reader.get_element("base-offset"), reader.get_element("arm-offset"));
}
catch (std::runtime_error &){
}
entire_x_rotation_offset_ = 0.0f;
entire_y_rotation_offset_ = 0.0f;
entire_z_rotation_offset_ = 0.0f;
}
DHDaVinciPoseGrabber::DHDaVinciPoseGrabber(const ConfigReader &reader, const std::string &output_dir) : BaseDaVinciPoseGrabber(reader, output_dir) {
self_name_ = "dh-davinci-grabber";
checkSelfName(reader.get_element("name"));
if (reader.get_element("joint") == "PSM1")
target_joint_ = davinci::DaVinciJoint::PSM1;
else if (reader.get_element("joint") == "PSM2")
target_joint_ = davinci::DaVinciJoint::PSM2;
else if (reader.get_element("joint") == "ECM")
target_joint_ = davinci::DaVinciJoint::ECM;
else
throw std::runtime_error("Error, bad joint");
switch (target_joint_){
case davinci::DaVinciJoint::PSM1:
num_base_joints_ = chain_.mSUJ1OriginSUJ1Tip.size();
num_arm_joints_ = chain_.mPSM1OriginPSM1Tip.size();
break;
case davinci::DaVinciJoint::PSM2:
num_base_joints_ = chain_.mSUJ2OriginSUJ2Tip.size();
num_arm_joints_ = chain_.mPSM2OriginPSM2Tip.size();
break;
case davinci::DaVinciJoint::ECM:
num_base_joints_ = chain_.mSUJ3OriginSUJ3Tip.size();
num_arm_joints_ = 4;//chain_.mECM1OriginECM1Tip.size();
break;
}
arm_offsets_ = std::vector<double>(num_arm_joints_, 0.0);
base_offsets_ = std::vector<double>(num_base_joints_, 0.0);
arm_joints_ = std::vector<double>(num_arm_joints_, 0.0);
base_joints_ = std::vector<double>(num_base_joints_, 0.0);
try{
SetupOffsets(reader.get_element("base-offset"), reader.get_element("arm-offset"));
}
catch (std::runtime_error &){
}
base_ifs_.open(reader.get_element("base-joint-file"));
if (!base_ifs_.is_open()){
throw std::runtime_error("Error, could not open file: " + reader.get_element("base-joint-file"));
}
base_ifs_.exceptions(std::ifstream::eofbit);
arm_ifs_.open(reader.get_element("arm-joint-file"));
if (!arm_ifs_.is_open()){
throw std::runtime_error("Error, could not open file: " + reader.get_element("arm-joint-file"));
}
arm_ifs_.exceptions(std::ifstream::eofbit);
base_ofs_file_ = output_dir + "/" + reader.get_element("output-base-joint-file");
arm_ofs_file_ = output_dir + "/" + reader.get_element("output-arm-joint-file");
try{
se3_ofs_file_ = output_dir + "/" + reader.get_element("output-se3-file");
}
catch (...){
se3_ofs_file_ = output_dir + "/" + reader.get_element("output-se3"); //stupid old code
}
}
QuaternionPoseGrabber::QuaternionPoseGrabber(const ConfigReader &reader, const std::string &output_dir) : SE3DaVinciPoseGrabber(reader, output_dir, false) {
self_name_ = "quaternion-pose-grabber";
checkSelfName(reader.get_element("name"));
}
