Status PolymetisControllerServerImpl::InitRobotClient()

in polymetis/src/polymetis_server.cpp [72:116]

• 31 lines of code
• 3 McCabe index (conditional complexity)

Status PolymetisControllerServerImpl::InitRobotClient(
    ServerContext *context, const RobotClientMetadata *robot_client_metadata,
    Empty *) {
  std::cout << "\n\n==== Initializing new RobotClient... ====" << std::endl;

  num_dofs_ = robot_client_metadata->dof();

  // Create initial state dictionary
  timestamp_ = torch::tensor(0.0);
  joint_pos_ = torch::zeros(num_dofs_);
  joint_vel_ = torch::zeros(num_dofs_);

  state_dict_.insert("timestamp", timestamp_);
  state_dict_.insert("joint_pos", joint_pos_);
  state_dict_.insert("joint_vel", joint_vel_);

  // Load default controller bytes into model buffer
  controller_model_buffer_.clear();
  std::string binary_blob = robot_client_metadata->default_controller();
  for (int i = 0; i < binary_blob.size(); i++) {
    controller_model_buffer_.push_back(binary_blob[i]);
  }

  // Load default controller from model buffer
  memstream model_stream(controller_model_buffer_.data(),
                         controller_model_buffer_.size());
  try {
    robot_client_context_.default_controller = torch::jit::load(model_stream);
  } catch (const c10::Error &e) {
    std::cerr << "error loading default controller:\n";
    std::cerr << e.msg() << std::endl;
    return Status::CANCELLED;
  }

  // Set URDF file of new context
  robot_client_context_.metadata = RobotClientMetadata(*robot_client_metadata);

  // Set last updated timestep of robot client context
  robot_client_context_.last_update_ns = getNanoseconds();

  resetControllerContext();

  std::cout << "Success.\n\n";
  return Status::OK;
}