예제 #1
0
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");

}
예제 #2
0
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)
  }

}
예제 #3
0
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;

}
예제 #4
0
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 
  }
  
}
예제 #5
0
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"));

}